A weighted rolling optimization decision method for virtual driving field in ice and snow environment

Abstract

The present invention provides a virtual driving field weight rolling optimization decision method in an ice and snow environment. First, a virtual driving field model is established, and a risk field model, a traction field model and a potential field model are respectively established according to the environment in which a smart car is located; then, the virtual field force is used as an external force acting on the smart car, and a nonlinear weight decision model considering vehicle dynamics is established; then, a virtual driving field weight decision method based on model predictive control is designed, and the weight value of each field force is decided by rolling solution of the optimization problem; finally, the optimized weight value is substituted into the nonlinear weight decision model for path planning; the method can effectively combine path planning and decision-making based on the virtual driving field, and can enable the smart car to react faster when facing obstacles.

Description

Technical Field

[0001] The invention belongs to the field of automobile driving control and relates to a weighted rolling optimization decision method for a virtual driving field in an ice and snow environment. Background Art

[0002] Autonomous driving is a current research hotspot. It mainly consists of four parts: environmental perception, behavioral decision-making, path planning, and motion control. Environmental perception is used to obtain, process, and fuse environmental information collected by sensors. Behavioral decision-making combines environmental information to output reasonable and safe driving behaviors. Path planning plans appropriate vehicle speeds and paths based on different driving behaviors. Motion control tracks vehicle speeds and paths. In most studies, the decision-making module and the planning module are connected based on driving behaviors such as lane changing, keeping, accelerating, and decelerating. Whenever the decision result changes, the path and speed need to be replanned. Due to frequent accidents in dynamic traffic environments, the sampling frequency of decision-making and planning needs to be increased to avoid emergencies. In actual use, incorrect driving behaviors may occur due to calculation delays. Summary of the invention

[0003] In order to better combine decision-making and planning, the present invention proposes a weighted rolling optimization decision-making method for a virtual driving field in an ice and snow environment;

[0004] The weighted rolling optimization decision method for a virtual driving field in an ice and snow environment described in the present invention is implemented by adopting the following technical solutions:

[0005] A weighted rolling optimization decision method for a virtual driving field in an ice and snow environment is provided. A smart car is defined as a vehicle that uses the decision proposed by the method, and there is only one smart car. In order to simulate the obstacle avoidance behavior of the smart car, a vehicle that can hinder the normal driving of the smart car is defined as an obstacle car. The method will not change the motion state of the obstacle car. The smart car has sensors that can locate its own position, speed, direction and road environment as well as the position of obstacles. The front vehicle of two vehicles traveling in the same direction is defined as the front vehicle, and the rear vehicle is defined as the rear vehicle. The two vehicles can be both obstacle vehicles, or one obstacle vehicle and one smart car. The XOY coordinate system is defined as the inertial coordinate system, and the Xoy coordinate system is defined as the vehicle body coordinate system. The position of the vehicle's center of mass is used when describing the vehicle position, and the distance between vehicles is the distance between the vehicle's centers of mass. The specific steps of the method are as follows:

[0006] Step 1: Create a virtual parking lot model:

[0007] According to the environment where the smart car is located, a virtual driving field model is established, including a risk field model, a traction field model and a potential field model;

[0008] The risk field is used to ensure driving safety. It uses an index to represent the degree of collision risk between the smart car and the obstacle car. When the distance between the smart car and the obstacle car exceeds the safety distance threshold, it is considered that there is no collision risk. The risk field potential energy function is established as follows:

[0009]

[0010]

[0011] Among them, U oi is the risk field potential energy function; ω1 is the risk field weight of the smart car; d(p e ,p oi ) is the distance between the smart car and the i-th obstacle car, in m, where i∈{1,2,…,n}, n is the number of obstacle cars, where n is a positive integer greater than 0; d0 is the safety distance threshold, in m; p e is the position of the smart car in the XOY coordinate system (X e ,Y e ) code, X e is the longitudinal displacement of the smart car in the XOY coordinate system, in meters; Y e is the lateral displacement of the smart car in the XOY coordinate system, in m; p oi is the position of the i-th obstacle vehicle in the XOY coordinate system (X oi ,Y oi ) code, X oi is the longitudinal displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters; Y oi is the lateral displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters;

[0012] Considering that the safety distance threshold will affect the risk field potential energy function, its value is related to the relative speed of the smart car and the obstacle car. Since the road adhesion coefficient in the ice and snow environment is low, the braking force of the smart car is also low, so a longer distance is required to ensure the driving safety of the smart car and the obstacle car in front of it. The expression of the safety distance threshold d0 is described as:

[0013]

[0014] Among them, v x is the longitudinal speed of the smart car in the xoy coordinate system, in m / s; v oxi is the longitudinal speed of the ith obstacle vehicle in the xoy coordinate system, in m / s; m is the mass of the smart vehicle, in kg; t d is the reaction time of the driver of the following vehicle, in seconds, with a range of 1:2 seconds; F max is the tire force limit of the smart car, in N; d safeIt is the minimum distance between the front and rear vehicles when they are stationary, in meters, with a range of 5:10 meters;

[0015] Considering that the road adhesion coefficient in ice and snow environments is lower than that of ordinary roads, in order to avoid skidding or side deviation, the braking force of the smart car is directly proportional to the road adhesion coefficient;

[0016]

[0017] Among them, F z is the vertical load of the smart car, in N; μ is the road adhesion coefficient;

[0018] The risk field force is obtained by using the gradient descent method to solve the risk field potential function. The direction of the risk field force is from the i-th obstacle car to the smart car:

[0019]

[0020] Among them, F oi is the risk field force, unit N;

[0021] The longitudinal risk field force and lateral risk field force are calculated by combining the angle between the i-th obstacle vehicle and the x-axis of the xoy coordinate system:

[0022]

[0023]

[0024]

[0025]

[0026] Among them, F oxi is the longitudinal risk field force generated by the i-th obstacle vehicle, in N; F oyi is the lateral risk field force generated by the i-th obstacle vehicle, in N; x oi is the longitudinal displacement of the i-th obstacle vehicle in the xoy coordinate system, in m; y oi is the lateral displacement of the i-th obstacle vehicle in the xoy coordinate system, in meters; is the yaw angle of the smart car, in rad; θ i is the angle between the field force generated by the i-th obstacle vehicle and the x-axis, in rad;

[0027] Since the longitudinal risk field force generated by the obstacle vehicle has a greater impact on the driving of the smart vehicle, the lateral force adjustment factor k is introduced into equation (7): y It is used to weaken the lateral risk field force generated by the obstacle vehicle, calculate the risk field force generated by all obstacle vehicles within the sensing range, and obtain the risk field model as follows:

[0028]

[0029]

[0030] Among them, k y is the lateral force adjustment factor, with a value of 0.5; F ox is the total longitudinal risk field force generated by the obstacle vehicle, unit N; F oy is the total longitudinal risk field force generated by the obstacle vehicle, unit N;

[0031] The traction field model is composed of the traction field force generated by the center line of the lane closest to the smart car and the traction field force generated by the expected speed of the smart car. The traction field force generated by the center line of the lane closest to the smart car represents that the smart car is expected to drive along the center line of the lane. Therefore, the traction field model of the lane center line is described as:

[0032]

[0033]

[0034] Where ω2 is the lane centerline traction field weight of the smart car; F cx The longitudinal traction force generated by the center line of the lane closest to the smart car, in N; F cy is the lateral traction force generated by the center line of the lane closest to the smart car, in N; y lane is the ordinate of the centerline of the lane closest to the smart car in the xoy coordinate system, in meters; X lane Y is the horizontal coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters; lane The vertical coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters;

[0035] The traction force generated by the expected speed of the smart car represents the expectation that the smart car can move forward at a certain speed. The expected speed traction force model is described as v * is the reference input, v x For feedback and a x The form of the P controller for the controlled quantity is:

[0036] F long =ma x (14)

[0037] a x =ω3(v * -v x ) (15)

[0038] Among them, ω3 is the expected speed traction field weight of the smart car; v * is the expected speed of the smart car, in m / s; ax is the longitudinal acceleration of the smart car in the xoy coordinate system, in m 2 / s;F long is the traction force generated by the expected speed of the smart car, in N;

[0039] In addition, the smart car generates lateral force when changing lanes or returning to the center, and a potential field model is established:

[0040] F lat =ω4F ymax (16)

[0041] Among them, ω4 is the potential field weight of the smart car; F ymax is the maximum lateral force of the smart car on the current road surface, in N, with a value of 6000N; F lat It is the potential field force generated by the smart car, in N;

[0042] Step 2: Establish a nonlinear weighted decision model considering vehicle dynamics:

[0043] Assuming that the smart car is a rigid body, according to the kinematic equations and geometric relationships, we get:

[0044]

[0045] Among them, v y is the lateral speed of the smart car in the xoy coordinate system, in m / s;

[0046] Decompose and classify the risk field forces, traction field forces and potential field forces in step 1 horizontally and vertically:

[0047]

[0048] Among them, F x is the longitudinal field force, unit N; F y is the lateral field force, unit N;

[0049] Taking the field force of the virtual driving field as the external force acting on the smart car, we get:

[0050]

[0051] In most cases, the smart car is in a stable state, and the lateral tire force of the smart car has not reached the saturation state. At this time, the tire force changes of the front and rear wheels of the smart car are in the linear region. The linear relationship between the front and rear wheel tire forces and the tire side slip angle of the smart car is established:

[0052]

[0053]

[0054] Among them, Fyf is the lateral force of the front wheel of the smart car, in N; F yr is the lateral force of the rear wheel of the smart car, in N; C f is the lateral stiffness of the front tire of the smart car, in N·rad; C r is the cornering stiffness of the rear tire of the smart car, in N·rad; α f is the side slip angle of the front tire of the smart car, in rad; α r is the side slip angle of the rear tire of the smart car, in rad; β is the side slip angle of the center of mass of the smart car, in rad; r is the yaw angular velocity of the smart car, in rad / s; δ f is the front wheel turning angle of the smart car, in rad; l f is the distance from the center of mass of the smart car to the front axle, in meters; l r is the distance from the center of mass of the smart car to the rear axle, in meters;

[0055] The lateral field force in the decomposed virtual driving field is equivalent to the resultant force along the y-axis received by the smart car, so there exists:

[0056] F y =F yf cosδ f +F yr (twenty two)

[0057] Since the front wheel turning angle of the smart car is small, cosδ f The approximate value is 1, so formula (22) is rewritten as:

[0058] F y =F yf +F yr (twenty three)

[0059] Combining equations (20)-(23) we can get the front wheel turning angle of the smart car:

[0060]

[0061] When performing lateral control, the longitudinal dynamics of the vehicle are ignored and the mathematical model of the vehicle yaw rate is obtained:

[0062]

[0063] Among them, I z is the moment of inertia of the smart car, in kg·m 2 ;

[0064] Substituting equation (24) into equation (25) yields the updated mathematical model of the vehicle yaw rate:

[0065]

[0066] Since the center of mass side slip angle of the smart car is small, it can be described as:

[0067]

[0068] Combining equations (17)-(26) we get the nonlinear weighted decision vehicle model:

[0069]

[0070] According to the state space equation shown in formula (28), the system control variable u, system state x and system output y are defined as:

[0071]

[0072] Step 3: Design a virtual parking lot weight decision method based on model predictive control:

[0073] Set the prediction time domain to P, the control time domain to N, and satisfy N≤P, and the discrete time to T s , taking the value of 0.2s, the forward difference method is used to discretize equation (28) to obtain the discrete model:

[0074]

[0075] Among them, the output matrix C d =diag(1;1;1;1;1;1),f k Represents the gradient of change of the system state quantity at time k:

[0076] The predicted output expression at the kth moment is:

[0077]

[0078] The P-step system state prediction equation is derived as follows:

[0079]

[0080] The collision time is used to characterize the collision risk between the smart car as the front car and the rear car, and the collision risk between the smart car as the rear car and the front car. The smaller the collision time, the higher the collision risk. Therefore, the collision time is expressed as follows:

[0081]

[0082] Where TTC is the collision time, in seconds; t danger is the dangerous time constant, unit is s, value is 0.01; t safe is the safety time constant, unit is s, value is 1000; d(pf ,p r ) is the distance between the front and rear vehicles, in meters; p f is the code for the position of the preceding vehicle in the XOY coordinate system; p r v is the code for the position of the rear vehicle in the XOY coordinate system; f is the speed of the front vehicle in the xoy coordinate system, in m / s; v r is the speed of the rear vehicle in the xoy coordinate system, in m / s;

[0083] The first condition for the smart car to drive safely is to keep a safe distance from the obstacle car. The distance is proportional to the collision time. Assuming that the lane where the smart car is located is lane R1, the lane closest to the smart car is lane R2. If the smart car drives along the center line of the lane, lane R2 is regarded as lane R1. Therefore, a safety objective function J1 is established, which is jointly described by the collision time between the smart car and the front car in lane R1 and the collision time between the smart car and the front car in lane R2. A safety objective function J2 is established, which is jointly described by the collision time between the smart car and the rear car in lane R1 and the collision time between the smart car and the rear car in lane R2:

[0084]

[0085]

[0086]

[0087] Among them, TTC R1f is the collision time between the smart car and the front car in R1 lane, in seconds; TTC R2f is the collision time between the smart car and the front car in R2 lane, in seconds; TTC R1r is the collision time between the smart car and the rear car in R1 lane, in seconds; TTC R2r is the collision time between the smart car and the rear car in lane R2, in seconds; α is the weighting factor; d is the lane width, in meters, with a value of 3.6 meters; j = 1, 2, …, P, where P is the prediction time domain;

[0088] In addition, it is hoped that the smart car can travel along the center line of the lane and maintain the expected speed. The comfort objective function J3 is established by combining formula (13), and the rapidity objective function J4 is established by combining formula (15):

[0089]

[0090]

[0091] Where, q = 1, 2, ..., N, N is the control time domain;

[0092] The system constraints include three parts: boundary constraints, tire force saturation constraints, and weight normalization constraints;

[0093] Since the smart car cannot go beyond the lane boundary, add road boundary constraints to the system:

[0094]

[0095] Among them, y e is the lateral displacement of the smart car in the xoy coordinate system, in m, with a value of 0; llane is the ordinate of the centerline of the leftmost lane in the xoy coordinate system, in meters; y rlane is the ordinate of the centerline of the rightmost lane in the xoy coordinate system, in meters; W is the width of the smart car, in meters, and the value is 2.2 meters;

[0096] y at time k llane and rlane It is expressed as:

[0097]

[0098] Among them, X llane Y is the horizontal coordinate of the center line of the leftmost lane in the XOY coordinate system, in meters; llane is the ordinate of the centerline of the leftmost lane in the XOY coordinate system, in meters; X rlane Y is the horizontal coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters; rlane The vertical coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters;

[0099] Since the lateral displacement of the smart car in the xoy coordinate system is always 0, equation (40) can be rewritten as:

[0100]

[0101] To prevent the smart car from slipping and rolling due to tire force saturation, add lateral force and longitudinal force constraints:

[0102] -F ymax <F oy +F cy +F lat <F ymax (43)

[0103] -F xmax <F ox +F cx +F long <F xmax (44) Among them, F xmax is the maximum longitudinal force of the smart car on the current road surface, in N, with a value of 6000N;

[0104] In order to keep the objective function in the same dimensional range, add a weight normalization constraint:

[0105]

[0106] Combining equations (35)-(45), the optimization problem can be described as:

[0107]

[0108] Among them, Γ1 is the weight factor of the objective function J1, Γ2 is the weight factor of the objective function J2, Γ3 is the weight factor of the objective function J3, and Γ4 is the weight factor of the objective function J4;

[0109] The mechanism of model predictive control is described as the following two steps. First, at time k, the current measurement value is used as the initial condition, and the future dynamics of the system are predicted by the discrete model of equation (30). The optimization problem of equation (46) is solved in a rolling manner to obtain the optimal control sequence:

[0110]

[0111] Then, the first element of the optimal control sequence Act on the path planning part of the virtual driving field; at time k+1, use the new measurement value as the initial condition to re-predict the future dynamics of the system, refresh the optimization problem and solve it again, and then get U * The first element u of (k+1) * (k+1|k+1) acts on the path planning part of the virtual driving field; repeating this process to achieve rolling optimization decision-making;

[0112] Step 4: Plan the virtual driving path:

[0113] The model of the virtual driving field path planning part is the same as the nonlinear weight decision vehicle model of equation (28). At time k, the first value of the optimal control sequence obtained by model predictive control is The nonlinear weighted decision vehicle model acting on Equation (28) is T d is the sampling time of the virtual driving field, taking the value as 0.05s, we get:

[0114]

[0115]

[0116] At time k+1, the optimal control sequence U obtained by model predictive control is *The first value of (k+1) is applied to the path planning part, and equations (48)-(49) are calculated, and the path planning is completed by repeating this process. Compared with the prior art, the beneficial effects of the present invention are:

[0117] Because this method establishes a framework structure for rolling optimization decision-making and virtual driving field path planning, and optimizes the weight of the field force in real time as the scene changes, the path planning and decision-making based on the virtual driving field can be effectively combined; because this method uses the virtual field force as the external force acting on the smart car, takes the weight as input, and takes the state information such as the position of the smart car as output, a nonlinear weight decision model considering vehicle dynamics is established; because this method uses the virtual driving field as the controlled object of the smart car decision-making, and the virtual driving field will fine-tune the vehicle's driving path according to the current environment, it can enable the smart car to respond faster when facing obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0118] The present invention will be further described below in conjunction with the accompanying drawings:

[0119] Figure 1 It is a flow chart of a weighted rolling optimization decision method for a virtual driving field in an ice and snow environment according to the present invention;

[0120] Figure 2 A schematic diagram of the forces acting on the smart car of the present invention in a virtual driving field;

[0121] Figure 3 is a schematic diagram of the smart car of the present invention serving as a front vehicle;

[0122] Figure 4 This is a schematic diagram of the smart car of the present invention serving as a rear vehicle; DETAILED DESCRIPTION

[0123] The present invention is described in detail below in conjunction with the accompanying drawings:

[0124] The present invention proposes a weighted rolling optimization decision method for a virtual driving field in an ice and snow environment, defines a smart car as a vehicle that uses the decision proposed by the method, and there is only one smart car. In order to simulate the obstacle avoidance behavior of the smart car, a vehicle that can hinder the normal driving of the smart car is defined as an obstacle vehicle. The method will not change the motion state of the obstacle vehicle. The smart car has sensors that can locate its own position, speed, direction and road environment as well as the position of obstacles. The front vehicle of two vehicles traveling in the same direction is defined as the front vehicle, and the rear vehicle is defined as the rear vehicle, wherein the two vehicles can be both obstacle vehicles, or one obstacle vehicle and the other smart car; the XOY coordinate system is defined as the inertial coordinate system, and the Xoy coordinate system is defined as the vehicle body coordinate system; the position of the vehicle's center of mass is used when describing the vehicle position, and the distance between vehicles is the distance between the vehicle's centers of mass; such as Figure 1As shown, the specific implementation steps are as follows:

[0125] Step 1: Create a virtual parking lot model:

[0126] According to the environment where the smart car is located, a virtual driving field model is established, including a risk field model, a traction field model and a potential field model;

[0127] Within a certain range, both static and dynamic obstacle vehicles will have an impact on the driving trajectory of the smart car, thereby establishing a risk field model; in the artificial potential field method, in order to limit the driving range of the smart car, the repulsive field generated by the lane boundary will be considered. In other words, the virtual force generated by the lane boundary is to enable the smart car to better drive along the center line of the lane, thereby establishing a lane centerline traction field model; on the other hand, in the artificial potential field method, in order to make the smart car approach the target point, a gravitational field of the target point to the smart car will be established. In most cases, there may be problems such as unreachable targets and local optimality. In the path planning of smart cars, In the face of a multi-lane driving environment, the driving target of the vehicle should not be limited to a certain point. The expected driving direction of the smart car can be used as the target for path planning. Therefore, the expected speed replaces the expected direction to attract the smart car to move forward, thereby establishing an expected speed traction field model; when there is a slow-moving obstacle vehicle in front of the smart car, in order to make the smart car change lanes to obtain higher driving efficiency, a lateral force is required to trigger the lane change. When the smart car changes lanes to the side lane, in order to prevent the smart car from continuing to change lanes to the side lane, a lateral force is required to balance the repulsive force generated by the obstacle vehicle, thereby establishing a potential field model;

[0128] The risk field is used to ensure driving safety. It uses an index to represent the degree of collision risk between the smart car and the obstacle car. When the distance between the smart car and the obstacle car exceeds the safety distance threshold, it is considered that there is no collision risk. The risk field potential energy function is established as follows:

[0129]

[0130]

[0131] Among them, U oi is the risk field potential energy function; ω1 is the risk field weight of the smart car; d(p e ,p oi ) is the distance between the smart car and the i-th obstacle car, in m, where i∈{1,2,…,n}, n is the number of obstacle cars, where n is a positive integer greater than 0; d0 is the safety distance threshold, in m; p e is the position of the smart car in the XOY coordinate system (X e ,Y e ) code, X eis the longitudinal displacement of the smart car in the XOY coordinate system, in meters; Y e is the lateral displacement of the smart car in the XOY coordinate system, in m; p oi is the position of the i-th obstacle vehicle in the XOY coordinate system (X oi ,Y oi ) code, X oi is the longitudinal displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters; Y oi is the lateral displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters;

[0132] Considering that the safety distance threshold will affect the risk field potential energy function, its value is related to the relative speed of the smart car and the obstacle car. Since the road adhesion coefficient in the ice and snow environment is low, the braking force of the smart car is also low, so a longer distance is required to ensure the driving safety of the smart car and the obstacle car in front of it. The expression of the safety distance threshold d0 is described as:

[0133]

[0134] Among them, v x is the longitudinal speed of the smart car in the xoy coordinate system, in m / s; v oxi is the longitudinal speed of the ith obstacle vehicle in the xoy coordinate system, in m / s; m is the mass of the smart vehicle, in kg; t d is the reaction time of the driver of the following vehicle, in seconds, with a range of 1:2 seconds; F max is the tire force limit of the smart car, in N; d safe It is the minimum distance between the front and rear vehicles when they are stationary, in meters, with a range of 5:10 meters;

[0135] Considering that the road adhesion coefficient in ice and snow environments is lower than that of ordinary roads, in order to avoid skidding or side deviation, the braking force of the smart car is directly proportional to the road adhesion coefficient;

[0136]

[0137] Among them, F z is the vertical load of the smart car, in N; μ is the road adhesion coefficient;

[0138] The risk field force is obtained by using the gradient descent method to solve the risk field potential function. The direction of the risk field force is from the i-th obstacle car to the smart car:

[0139]

[0140] Among them, F oi is the risk field force, unit N;

[0141] The longitudinal risk field force and lateral risk field force are calculated by combining the angle between the i-th obstacle vehicle and the x-axis of the xoy coordinate system:

[0142]

[0143]

[0144]

[0145]

[0146] Among them, F oxi is the longitudinal risk field force generated by the i-th obstacle vehicle, in N; F oyi is the lateral risk field force generated by the i-th obstacle vehicle, in N; x oi is the longitudinal displacement of the i-th obstacle vehicle in the xoy coordinate system, in m; y oi is the lateral displacement of the i-th obstacle vehicle in the xoy coordinate system, in meters; is the yaw angle of the smart car, in rad; θ i is the angle between the field force generated by the i-th obstacle vehicle and the x-axis, in rad;

[0147] Since the longitudinal risk field force generated by the obstacle vehicle has a greater impact on the driving of the smart vehicle, the lateral force adjustment factor k is introduced into equation (7): y It is used to weaken the lateral risk field force generated by the obstacle vehicle, calculate the risk field force generated by all obstacle vehicles within the sensing range, and obtain the risk field model as follows:

[0148]

[0149]

[0150] Among them, k y is the lateral force adjustment factor, with a value of 0.5; F ox is the total longitudinal risk field force generated by the obstacle vehicle, unit N; F oy is the total longitudinal risk field force generated by the obstacle vehicle, unit N;

[0151] The traction field model is composed of the traction field force generated by the center line of the lane closest to the smart car and the traction field force generated by the expected speed of the smart car. The traction field force generated by the center line of the lane closest to the smart car represents that the smart car is expected to drive along the center line of the lane. Therefore, the traction field model of the lane center line is described as:

[0152]

[0153]

[0154] Where ω2 is the lane centerline traction field weight of the smart car; F cx The longitudinal traction force generated by the center line of the lane closest to the smart car, in N; F cy is the lateral traction force generated by the center line of the lane closest to the smart car, in N; y lane is the ordinate of the centerline of the lane closest to the smart car in the xoy coordinate system, in meters; X lane Y is the horizontal coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters; lane The vertical coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters;

[0155] The traction force generated by the expected speed of the smart car represents the expectation that the smart car can move forward at a certain speed, replacing the gravitational field generated by the target point in the traditional artificial potential field, which can effectively avoid the problem of unreachable targets. The expected speed traction field model is described as v * is the reference input, v x For feedback and a x The form of the P controller for the controlled quantity is:

[0156] F long =ma x (14)

[0157] a x =ω3(v * -v x ) (15)

[0158] Among them, ω3 is the expected speed traction field weight of the smart car; v * is the expected speed of the smart car, in m / s; a x is the longitudinal acceleration of the smart car in the xoy coordinate system, in m 2 / s;F long is the traction force generated by the expected speed of the smart car, in N;

[0159] In addition, the smart car generates lateral force when changing lanes or returning to the center, and a potential field model is established:

[0160] F lat =ω4F ymax (16)

[0161] Among them, ω4 is the potential field weight of the smart car; F ymax is the maximum lateral force of the smart car on the current road surface, in N, with a value of 6000N; F lat It is the potential field force generated by the smart car, in N;

[0162] Step 2: Establish a nonlinear weighted decision model considering vehicle dynamics:

[0163] Assuming that the smart car is a rigid body, according to the kinematic equations and geometric relationships, we get:

[0164]

[0165] Among them, v y is the lateral speed of the smart car in the xoy coordinate system, in m / s;

[0166] like Figure 2 As shown in the figure, the risk field force, traction field force and potential field force in step 1 are decomposed and classified horizontally and vertically. The risk field generates longitudinal force and lateral force on the smart car to keep a safe distance from the obstacle car; the lane centerline traction field generates longitudinal force and lateral force on the smart car to make the smart car drive along the lane centerline; the expected speed traction field generates longitudinal force on the smart car to keep the smart car moving forward; the potential field generates lateral force on the smart car to enable the smart car to successfully complete the lane change and return action; therefore, the field force can be described as:

[0167]

[0168] Among them, F x is the longitudinal field force, unit N; F y is the lateral field force, unit N;

[0169] Taking the field force of the virtual driving field as the external force acting on the smart car, we get:

[0170]

[0171] In most cases, the smart car is in a stable state, and the lateral tire force of the smart car has not reached the saturation state. At this time, the tire force changes of the front and rear wheels of the smart car are in the linear region. The linear relationship between the front and rear wheel tire forces and the tire side slip angle of the smart car is established:

[0172]

[0173]

[0174] Among them, F yf is the lateral force of the front wheel of the smart car, in N; F yr is the lateral force of the rear wheel of the smart car, in N; C f is the lateral stiffness of the front tire of the smart car, in N·rad; C r is the cornering stiffness of the rear tire of the smart car, in N·rad; α f is the side slip angle of the front tire of the smart car, in rad; α ris the side slip angle of the rear tire of the smart car, in rad; β is the side slip angle of the center of mass of the smart car, in rad; r is the yaw angular velocity of the smart car, in rad / s; δ f is the front wheel turning angle of the smart car, in rad; l f is the distance from the center of mass of the smart car to the front axle, in meters; l r is the distance from the center of mass of the smart car to the rear axle, in meters;

[0175] The lateral field force in the decomposed virtual driving field is equivalent to the resultant force along the y-axis received by the smart car, so there exists:

[0176] F y =F yf cosδ f +F yr (twenty two)

[0177] Since the front wheel turning angle of the smart car is small, cosδ f The approximate value is 1, so formula (22) is rewritten as:

[0178] F y =F yf +F yr (twenty three)

[0179] Combining equations (20)-(23) we can get the front wheel turning angle of the smart car:

[0180]

[0181] When performing lateral control, the longitudinal dynamics of the vehicle are ignored and the mathematical model of the vehicle yaw rate is obtained:

[0182]

[0183] Among them, I z is the moment of inertia of the smart car, in kg·m 2 ;

[0184] Substituting equation (24) into equation (25) yields the updated mathematical model of the vehicle yaw rate:

[0185]

[0186] Since the center of mass side slip angle of the smart car is small, it can be described as:

[0187]

[0188] Combining equations (17)-(26) we get the nonlinear weighted decision vehicle model:

[0189]

[0190] According to the state space equation shown in formula (28), the system control variable u, system state x and system output y are defined as:

[0191]

[0192] Step 3: Design a virtual parking lot weight decision method based on model predictive control:

[0193] Set the prediction time domain to P, the control time domain to N, and satisfy N≤P. For example, P is 16, N is 8, and the discrete time is T. s , taking the value of 0.2s, the forward difference method is used to discretize equation (28) to obtain the discrete model:

[0194]

[0195] Among them, the output matrix C d =diag(1;1;1;1;1;1),f k Represents the gradient of change of the system state quantity at time k:

[0196] The predicted output expression at the kth moment is:

[0197]

[0198] The P-step system state prediction equation is derived as follows:

[0199]

[0200] The collision time is used to characterize the collision risk between the smart car as the front car and the rear car, such as Figure 3 As shown in , and the collision risk between the smart car as the rear car and its front car, as shown in Figure 4 As shown, the smaller the collision time, the higher the collision risk;

[0201] When the distance between the front vehicle and the rear vehicle is less than the safety distance threshold, there is a collision risk, and the collision time at this time is represented by a smaller constant; when the distance between the front vehicle and the rear vehicle is greater than the safety distance threshold, if the speed of the rear vehicle is lower than that of the front vehicle, there is no collision risk, and the collision time at this time is represented by a larger constant. If the speed of the rear vehicle is greater than that of the front vehicle, the collision time at this time is related to the relative speed and the distance between the vehicles; because the road adhesion coefficient in the ice and snow environment is low, the braking force of the smart car is also low, so a larger safety distance threshold is required to ensure driving safety, so the collision time is expressed as follows:

[0202]

[0203] Where TTC is the collision time, in seconds; t danger is the dangerous time constant, unit is s, value is 0.01; t safe is the safety time constant, unit is s, value is 1000; d(p f ,p r ) is the distance between the front and rear vehicles, in meters; p f is the code for the position of the preceding vehicle in the XOY coordinate system; p r v is the code for the position of the rear vehicle in the XOY coordinate system; f is the speed of the front vehicle in the xoy coordinate system, in m / s; v r is the speed of the rear vehicle in the xoy coordinate system, in m / s;

[0204] The first condition for the smart car to drive safely is to keep a safe distance from the obstacle car. The distance is proportional to the collision time. Assuming that the lane where the smart car is located is lane R1, the lane closest to the smart car is lane R2. If the smart car drives along the center line of the lane, lane R2 is regarded as lane R1. Therefore, a safety objective function J1 is established, which is jointly described by the collision time between the smart car and the front car in lane R1 and the collision time between the smart car and the front car in lane R2. A safety objective function J2 is established, which is jointly described by the collision time between the smart car and the rear car in lane R1 and the collision time between the smart car and the rear car in lane R2:

[0205]

[0206]

[0207]

[0208] Among them, TTC R1f is the collision time between the smart car and the front car in R1 lane, in seconds; TTC R2f is the collision time between the smart car and the front car in R2 lane, in seconds; TTC R1r is the collision time between the smart car and the rear car in R1 lane, in seconds; TTC R2r is the collision time between the smart car and the rear car in lane R2, in seconds; α is the weighting factor; d is the lane width, in meters, with a value of 3.6 meters; j = 1, 2, …, P, where P is the prediction time domain;

[0209] In addition, it is hoped that the smart car can travel along the center line of the lane and maintain the expected speed. The comfort objective function J3 is established by combining formula (13), and the rapidity objective function J4 is established by combining formula (15):

[0210]

[0211]

[0212] Where, q = 1, 2, ..., N, N is the control time domain;

[0213] The system constraints include three parts: boundary constraints, tire force saturation constraints, and weight normalization constraints;

[0214] Since the smart car cannot go beyond the lane boundary, add road boundary constraints to the system:

[0215]

[0216] Among them, y e is the lateral displacement of the smart car in the xoy coordinate system, in m, with a value of 0; llane is the ordinate of the centerline of the leftmost lane in the xoy coordinate system, in meters; y rlane is the ordinate of the centerline of the rightmost lane in the xoy coordinate system, in meters; W is the width of the smart car, in meters, and the value is 2.2 meters;

[0217] y at time k llane and rlane It is expressed as:

[0218]

[0219] Among them, X llane Y is the horizontal coordinate of the center line of the leftmost lane in the XOY coordinate system, in meters; llane is the ordinate of the centerline of the leftmost lane in the XOY coordinate system, in meters; X rlane Y is the horizontal coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters; rlane The vertical coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters;

[0220] Since the lateral displacement of the smart car in the xoy coordinate system is always 0, equation (40) can be rewritten as:

[0221]

[0222] To prevent the smart car from slipping and rolling due to tire force saturation, add lateral force and longitudinal force constraints:

[0223] -F ymax <F oy +F cy +F lat <F ymax (43)

[0224] -F xmax <F ox +F cx +Flong <F xmax (44) Among them, F xmax is the maximum longitudinal force of the smart car on the current road surface, in N, with a value of 6000N;

[0225] In order to keep the objective function in the same dimensional range, add a weight normalization constraint:

[0226]

[0227] Combining equations (35)-(45), the optimization problem can be described as:

[0228]

[0229] Among them, Γ1 is the weight factor of the objective function J1, Γ2 is the weight factor of the objective function J2, Γ3 is the weight factor of the objective function J3, and Γ4 is the weight factor of the objective function J4;

[0230] The mechanism of model predictive control is described as the following two steps. First, at time k, the current measurement value is used as the initial condition, and the future dynamics of the system are predicted by the discrete model of equation (30). The optimization problem of equation (46) is solved in a rolling manner to obtain the optimal control sequence:

[0231]

[0232] Then, the first element of the optimal control sequence Act on the path planning part of the virtual driving field; at time k+1, use the new measurement value as the initial condition to re-predict the future dynamics of the system, refresh the optimization problem and solve it again, and then get U * The first element u of (k+1) * (k+1|k+1) acts on the path planning part of the virtual driving field; repeating this process to achieve rolling optimization decision-making;

[0233] Step 4: Plan the virtual driving path:

[0234] The model of the virtual driving field path planning part is the same as the nonlinear weight decision vehicle model of equation (28). At time k, the first value of the optimal control sequence obtained by model predictive control is The nonlinear weighted decision vehicle model acting on Equation (28) is T d is the sampling time of the virtual driving field, taking the value as 0.05s, we get:

[0235]

[0236]

[0237] At time k+1, the optimal control sequence U obtained by model predictive control is * The first value of (k+1) is applied to the path planning part to calculate equations (48)-(49), and this is repeated to complete the path planning.

Claims

1. A weighted rolling optimization decision method for a virtual driving field in an ice and snow environment, wherein a smart car is defined as a vehicle that uses the decision proposed by the method, and there is only one smart car. In order to simulate the obstacle avoidance behavior of the smart car, a vehicle that can hinder the normal driving of the smart car is defined as an obstacle car. The method will not change the motion state of the obstacle car. The smart car has sensors that can locate its own position, speed, direction and road environment as well as the position of obstacles. The front vehicle of two vehicles traveling in the same direction is defined as the front vehicle, and the rear vehicle is defined as the rear vehicle, wherein the two vehicles can be both obstacle vehicles, or one obstacle vehicle and one smart car; the XOY coordinate system is defined as the inertial coordinate system, and the Xoy coordinate system is defined as the vehicle body coordinate system; the position of the vehicle's center of mass is used when describing the vehicle position, and the distance between vehicles is the distance between the vehicle's centers of mass; the characteristics are as follows: The specific steps of this method are as follows: Step 1: Create a virtual parking lot model: According to the environment where the smart car is located, a virtual driving field model is established, including a risk field model, a traction field model and a potential field model; The risk field is used to ensure driving safety. It uses an index to represent the degree of collision risk between the smart car and the obstacle car. When the distance between the smart car and the obstacle car exceeds the safety distance threshold, it is considered that there is no collision risk. The risk field potential energy function is established as follows: Among them, U oi is the risk field potential energy function; ω1 is the risk field weight of the smart car; d(p e ,p oi ) is the distance between the smart car and the i-th obstacle car, in m, where i∈{1,2,…,n}, n is the number of obstacle cars, where n is a positive integer greater than 0; d0 is the safety distance threshold, in m; p e is the position of the smart car in the XOY coordinate system (X e ,Y e ) code, X e is the longitudinal displacement of the smart car in the XOY coordinate system, in meters; Y e is the lateral displacement of the smart car in the XOY coordinate system, in m; p oi is the position of the i-th obstacle vehicle in the XOY coordinate system (X oi ,Y oi ) code, X oi is the longitudinal displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters; Y oi is the lateral displacement of the i-th obstacle vehicle in the XOY coordinate system, in meters; Considering that the safety distance threshold will affect the risk field potential energy function, its value is related to the relative speed of the smart car and the obstacle car. Since the road adhesion coefficient in the ice and snow environment is low, the braking force of the smart car is also low, so a longer distance is required to ensure the driving safety of the smart car and the obstacle car in front of it. The expression of the safety distance threshold d0 is described as: Among them, v x is the longitudinal speed of the smart car in the xoy coordinate system, in m / s; v oxi is the longitudinal speed of the ith obstacle vehicle in the xoy coordinate system, in m / s; m is the mass of the smart vehicle, in kg; t d is the reaction time of the driver of the following vehicle, in seconds, with a range of 1:2 seconds; F max is the tire force limit of the smart car, in N; d safe It is the minimum distance between the front and rear vehicles when they are stationary, in meters, with a range of 5:10 meters; Considering that the road adhesion coefficient in ice and snow environments is lower than that of ordinary roads, in order to avoid skidding or side deviation, the braking force of the smart car is directly proportional to the road adhesion coefficient; Among them, F z is the vertical load of the smart car, in N; μ is the road adhesion coefficient; The risk field force is obtained by using the gradient descent method to solve the risk field potential function. The direction of the risk field force is from the i-th obstacle car to the smart car: Among them, F oi is the risk field force, unit N; The longitudinal risk field force and lateral risk field force are calculated by combining the angle between the i-th obstacle vehicle and the x-axis of the xoy coordinate system: Among them, F oxi is the longitudinal risk field force generated by the i-th obstacle vehicle, in N; F oyi is the lateral risk field force generated by the i-th obstacle vehicle, in N; x oi is the longitudinal displacement of the i-th obstacle vehicle in the xoy coordinate system, in m; y oi is the lateral displacement of the i-th obstacle vehicle in the xoy coordinate system, in meters; is the yaw angle of the smart car, in rad; θ i is the angle between the field force generated by the i-th obstacle vehicle and the x-axis, in rad; Since the longitudinal risk field force generated by the obstacle vehicle has a greater impact on the driving of the smart vehicle, the lateral force adjustment factor k is introduced into equation (7): y It is used to weaken the lateral risk field force generated by the obstacle vehicle, calculate the risk field force generated by all obstacle vehicles within the sensing range, and obtain the risk field model as follows: Among them, k y is the lateral force adjustment factor, with a value of 0.5; F ox is the total longitudinal risk field force generated by the obstacle vehicle, unit N; F oy is the total longitudinal risk field force generated by the obstacle vehicle, unit N; The traction field model is composed of the traction field force generated by the center line of the lane closest to the smart car and the traction field force generated by the expected speed of the smart car. The traction field force generated by the center line of the lane closest to the smart car represents that the smart car is expected to drive along the center line of the lane. Therefore, the traction field model of the lane center line is described as: Where ω2 is the lane centerline traction field weight of the smart car; F cx The longitudinal traction force generated by the center line of the lane closest to the smart car, in N; F cy is the lateral traction force generated by the center line of the lane closest to the smart car, in N; y lane is the ordinate of the centerline of the lane closest to the smart car in the xoy coordinate system, in meters; X lane Y is the horizontal coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters; lane The vertical coordinate of the center line of the lane closest to the smart car in the XOY coordinate system, in meters; The traction force generated by the expected speed of the smart car represents the expectation that the smart car can move forward at a certain speed. The expected speed traction force model is described as v * is the reference input, v x For feedback and a x The form of the P controller for the controlled quantity is: F long =at x (14) a x =ω3(v * -v x ) (15) Among them, ω3 is the expected speed traction field weight of the smart car; v * is the expected speed of the smart car, in m / s; a x is the longitudinal acceleration of the smart car in the xoy coordinate system, in m 2 / s;F long is the traction force generated by the expected speed of the smart car, in N; In addition, the smart car generates lateral force when changing lanes or returning to the center, and a potential field model is established: F lat =ω4F ymax (16) Among them, ω4 is the potential field weight of the smart car; F ymax is the maximum lateral force of the smart car on the current road surface, in N, with a value of 6000N; F lat It is the potential field force generated by the smart car, in N; Step 2: Establish a nonlinear weighted decision model considering vehicle dynamics: Assuming that the smart car is a rigid body, according to the kinematic equations and geometric relationships, we get: Among them, v y is the lateral speed of the smart car in the xoy coordinate system, in m / s; Decompose and classify the risk field forces, traction field forces and potential field forces in step 1 horizontally and vertically: Among them, F x is the longitudinal field force, unit N; F y is the lateral field force, unit N; Taking the field force of the virtual driving field as the external force acting on the smart car, we get: In most cases, the smart car is in a stable state, and the lateral tire force of the smart car has not reached the saturation state. At this time, the tire force changes of the front and rear wheels of the smart car are in the linear region. The linear relationship between the front and rear wheel tire forces and the tire side slip angle of the smart car is established: Among them, F yf is the lateral force of the front wheel of the smart car, in N; F yr is the lateral force of the rear wheel of the smart car, in N; C f is the lateral stiffness of the front tire of the smart car, in N·rad; C r is the cornering stiffness of the rear tire of the smart car, in N·rad; α f is the side slip angle of the front tire of the smart car, in rad; α r is the side slip angle of the rear tire of the smart car, in rad; β is the side slip angle of the center of mass of the smart car, in rad; r is the yaw angular velocity of the smart car, in rad / s; δ f is the front wheel turning angle of the smart car, in rad; l f is the distance from the center of mass of the smart car to the front axle, in meters; l r is the distance from the center of mass of the smart car to the rear axle, in meters; The lateral field force in the decomposed virtual driving field is equivalent to the resultant force along the y-axis received by the smart car, so there exists: F y =F yf cosδ f +F yr (22) Since the front wheel turning angle of the smart car is small, cosδ f The approximate value is 1, so formula (22) is rewritten as: F y =F yf +F yr (23) Combining equations (20)-(23) we can get the front wheel turning angle of the smart car: When performing lateral control, the longitudinal dynamics of the vehicle are ignored and the mathematical model of the vehicle yaw rate is obtained: Among them, I z is the moment of inertia of the smart car, in kg·m 2 ; Substituting equation (24) into equation (25) yields the updated mathematical model of the vehicle yaw rate: Since the center of mass side slip angle of the smart car is small, it can be described as: Combining equations (17)-(26) we get the nonlinear weighted decision vehicle model: According to the state space equation shown in formula (28), the system control variable u, system state x and system output y are defined as: Step 3: Design a virtual parking lot weight decision method based on model predictive control: Set the prediction time domain to P, the control time domain to N, and satisfy N≤P, and the discrete time to T s , taking the value of 0.2s, the forward difference method is used to discretize equation (28) to obtain the discrete model: Among them, the output matrix C d =diag(1;1;1;1;1;1),f k Represents the gradient of change of the system state quantity at time k: The predicted output expression at the kth moment is: The P-step system state prediction equation is derived as follows: The collision time is used to characterize the collision risk between the smart car as the front car and the rear car, and the collision risk between the smart car as the rear car and the front car. The smaller the collision time, the higher the collision risk. Therefore, the collision time is expressed as follows: Where TTC is the collision time, in seconds; t danger is the dangerous time constant, unit is s, value is 0.01; t safe is the safety time constant, unit is s, value is 1000; d(p f ,p r ) is the distance between the front and rear vehicles, in meters; p f is the code for the position of the preceding vehicle in the XOY coordinate system; p r v is the code for the position of the rear vehicle in the XOY coordinate system; f is the speed of the front vehicle in the xoy coordinate system, in m / s; v r is the speed of the rear vehicle in the xoy coordinate system, in m / s; The first condition for the smart car to drive safely is to keep a safe distance from the obstacle car. The distance is proportional to the collision time. Assuming that the lane where the smart car is located is lane R1, the lane closest to the smart car is lane R2. If the smart car drives along the center line of the lane, lane R2 is regarded as lane R1. Therefore, a safety objective function J1 is established, which is jointly described by the collision time between the smart car and the front car in lane R1 and the collision time between the smart car and the front car in lane R2. A safety objective function J2 is established, which is jointly described by the collision time between the smart car and the rear car in lane R1 and the collision time between the smart car and the rear car in lane R2: Among them, TTC R1f is the collision time between the smart car and the front car in R1 lane, in seconds; TTC R2f is the collision time between the smart car and the front car in R2 lane, in seconds; TTC R1r is the collision time between the smart car and the rear car in R1 lane, in seconds; TTC R2r is the collision time between the smart car and the rear car in lane R2, in seconds; α is the weighting factor; d is the lane width, in meters, with a value of 3.6 meters; j = 1, 2, …, P, where P is the prediction time domain; In addition, it is hoped that the smart car can travel along the center line of the lane and maintain the expected speed. The comfort objective function J3 is established by combining formula (13), and the rapidity objective function J4 is established by combining formula (15): Where, q = 1, 2, ..., N, N is the control time domain; The system constraints include three parts: boundary constraints, tire force saturation constraints, and weight normalization constraints; Since the smart car cannot go beyond the lane boundary, add road boundary constraints to the system: Among them, y e is the lateral displacement of the smart car in the xoy coordinate system, in m, with a value of 0; llane is the ordinate of the centerline of the leftmost lane in the xoy coordinate system, in meters; y rlane is the ordinate of the centerline of the rightmost lane in the xoy coordinate system, in meters; W is the width of the smart car, in meters, and the value is 2.2 meters; y at time k llane and rlane It is expressed as: Among them, X llane Y is the horizontal coordinate of the center line of the leftmost lane in the XOY coordinate system, in meters; llane is the ordinate of the centerline of the leftmost lane in the XOY coordinate system, in meters; X rlane Y is the horizontal coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters; rlane The vertical coordinate of the center line of the rightmost lane in the XOY coordinate system, in meters; Since the lateral displacement of the smart car in the xoy coordinate system is always 0, equation (40) can be rewritten as: To prevent the smart car from slipping and rolling due to tire force saturation, add lateral force and longitudinal force constraints: -F ymax <F oy +F cy +F lat <F ymax (43) -F xmax <F ox +F cx +F long <F xmax (44) Among them, F xmax is the maximum longitudinal force of the smart car on the current road surface, in N, with a value of 6000N; In order to keep the objective function in the same dimensional range, add a weight normalization constraint: Combining equations (35)-(45), the optimization problem can be described as: Among them, Γ1 is the weight factor of the objective function J1, Γ2 is the weight factor of the objective function J2, Γ3 is the weight factor of the objective function J3, and Γ4 is the weight factor of the objective function J4; The mechanism of model predictive control is described as the following two steps. First, at time k, the current measurement value is used as the initial condition, and the future dynamics of the system are predicted by the discrete model of equation (30). The optimization problem of equation (46) is solved in a rolling manner to obtain the optimal control sequence: Then, the first element of the optimal control sequence Act on the path planning part of the virtual driving field; at time k+1, use the new measurement value as the initial condition to re-predict the future dynamics of the system, refresh the optimization problem and solve it again, and then get U * The first element u of (k+1) * (k+1|k+1) acts on the path planning part of the virtual driving field; repeating this process to achieve rolling optimization decision-making; Step 4: Plan the virtual driving path: The model of the virtual driving field path planning part is the same as the nonlinear weight decision vehicle model of equation (28). At time k, the first value of the optimal control sequence obtained by model predictive control is The nonlinear weighted decision vehicle model acting on Equation (28) is T d is the sampling time of the virtual driving field, taking the value as 0.05s, we get: At time k+1, the optimal control sequence U obtained by model predictive control is * The first value of (k+1) is applied to the path planning part to calculate equations (48)-(49), and this is repeated to complete the path planning.

Images