A path planning method and device based on a force field

Abstract

The present invention relates to the technical field of path planning for autonomous driving of vehicles, and particularly relates to a path planning method and device based on a force field. The present invention proposes an intelligent path planning method that combines an adaptive force field model and an optimal trajectory generation method. In order to represent the risk function of a moving object, a fixed headway time strategy is adopted to design the force field of environmental vehicles. The proposed intelligent path planning method is applicable to various road autonomous driving situations, such as lane keeping, lane change, collision avoidance, etc. The adaptive force field model proposed by the present invention overcomes such defects by changing the magnitude of the risk. The cost function is optimally designed by comprehensively considering the performance of each part to generate an optimal trajectory.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning for vehicle autonomous driving, and particularly to a path planning method and device based on a force field. Background Art

[0002] With the development of road autonomous driving technology, higher requirements are put forward for the path planning of intelligent vehicles. In the existing related technologies, when performing path planning, for an obstacle environment, the common processing method is to abstract it into a mathematical model, and use algorithm models such as artificial intelligence to describe the relationship between driving states, driving behaviors, and traffic scene features. However, most of these common path planning algorithms solve the path planning problems in static obstacle or static scenarios.

[0003] In the existing technologies, CN110471421A discloses a path planning method and a path planning system for safe vehicle driving; CN109000651A discloses a path planning method and a path planning device. The existing force field models do not consider enough the dynamic aspects of obstacles. Summary of the Invention

[0004] In view of the above problems, the present invention provides a path planning method and device based on a force field.

[0005] A path planning method based on a force field, the method includes:

[0006] According to real-time ego-vehicle information, real-time environmental vehicle information, and real-time road information, establish a risk model, generate a trajectory set in the Frenet coordinate system, and screen out the optimal trajectory from the trajectory set;

[0007] Control the vehicle to move forward according to the optimal trajectory.

[0008] Further, the establishing a risk model, generating a trajectory set in the Frenet coordinate system, and screening out the optimal trajectory from the trajectory set includes:

[0009] Establish a force field model for road autonomous driving to describe driving risks;

[0010] Generate a trajectory set in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego-vehicle information;

[0011] Design a cost function to select the optimal trajectory, which is used for trajectory tracking after coordinate transformation.

[0012] Further, the establishing a force field model for road autonomous driving includes an environmental vehicle force field;

[0013] In the Frenet coordinate system, adopt a fixed headway strategy to establish an environmental vehicle force field model;

[0014] The change in the degree of danger in the s direction (ordinate) is mainly determined by the shape coefficient of the force field σ vehs , 2σ vehs represents the safe distance between the ego vehicle and the surrounding vehicle in the Frenet coordinate system, and the fixed headway at the current moment is adopted Set the reference distance between the ego vehicle and the surrounding vehicle, where D0 and τ are constants greater than 0. Therefore,

[0015] The force field of the i-th surrounding vehicle at time k is expressed as:

[0016] k = t + 1,…, t + N p , where t is the current time, k is the forward prediction time, and the interval between t and k is the prediction horizon N p ;

[0017] where Pveh_i represents the force field of the i-th surrounding vehicle; A veh represents the maximum value of the vehicle force field; where s and d represent the abscissa and ordinate of the ego vehicle in the Frenet coordinate system; where sveh_i and d veh_i represent the abscissa and ordinate of the i-th surrounding vehicle in the Frenet coordinate system; where c represents the coefficient that determines the shape of the force field of the i-th surrounding vehicle; N p represents the prediction horizon.

[0018] Furthermore, the establishment of the road autonomous driving force field model also includes the road boundary force field, which is expressed as follows:

[0019] m = 1, 2

[0020] where P rb_j represents the force field of the m-th road boundary; A rb represents the maximum value of the road boundary force field; yr, rb_m represents the lateral distance to the m-th road boundary in the earth coordinate system; where σ r b represents the road boundary force field coefficient.

[0021] Furthermore, the establishment of the road autonomous driving force field model also includes the lane centerline force field, which is expressed as follows:

[0022]

[0023] P ctr_n represents the force field of the n-th lane centerline; A ctr represents the maximum value of the lane line force field; y r,ctr_nrepresents the lateral distance to the centerline of the nth lane in the geodetic coordinate system; σ ctr represents the force field coefficient of the lane centerline.

[0024] Furthermore, generating a trajectory set in the Frenet coordinate system according to the environmental vehicle information, real-time road information, and real-time ego-vehicle information includes calculating the longitudinal velocity constraint, trajectory curvature constraint, and longitudinal and lateral acceleration constraints of the trajectory;

[0025] Calculating the longitudinal velocity constraint of the trajectory specifically includes:

[0026] Considering the comfort and safety of the driver, the speed in the s direction has a limit, expressed as:

[0027]

[0028] where represents the maximum lateral acceleration after considering ride comfort; k is the road curvature; is the speed limit in the s direction

[0029] When the road is straight, the speed limit is very large, and a fixed speed value is given The reference speed in the s direction is set to

[0030] Calculating the trajectory curvature constraint specifically includes:

[0031] After the lateral trajectory and the longitudinal trajectory are combined, it is necessary to check the trajectory curvature constraint k cand mainly considering the vehicle steering limit, k cand ∈[k min , k max , between the maximum and minimum values of the road curvature;

[0032] Calculating the longitudinal and lateral acceleration constraints of the trajectory specifically includes:

[0033] When considering the physical limitations of vehicle dynamics, it is necessary to constrain the longitudinal acceleration and lateral acceleration;

[0034]

[0035] where s(t) and d(t) are the ordinate and abscissa of the vehicle at time t in the Frenet coordinate system; a max is the maximum vehicle acceleration.

[0036] Further, generating a trajectory set in the Frenet coordinate system based on the environmental vehicle information, real-time road information, and real-time ego-vehicle information further includes generating a candidate path, i.e., a trajectory set; the generated trajectory set is decomposed horizontally and longitudinally. The horizontal trajectory is generated using a fourth-order polynomial, and the vertical trajectory is generated using a fifth-order polynomial. Finally, the obtained trajectory set is synthesized by combining the horizontal and vertical trajectories. Specifically, it includes:

[0037] Generate the horizontal trajectory using a fourth-order polynomial

[0038] s(t) = α0 + α1t + α2t 2 + α3t 3 + α4t 4

[0039] To solve for the 5 coefficients, state variables are required That is, the longitudinal displacement, longitudinal velocity, and longitudinal acceleration at the initial moment, and the longitudinal velocity and longitudinal acceleration at the end moment.

[0040] To generate different trajectories, the constraint from moment m to moment n is defined as

[0041] T n Indicates the time interval

[0042] When the ego-vehicle and the leading vehicle are in the same lane, if max(P veh_i (k|t)) > P veh，thres

[0043] Then Otherwise,

[0044] Is the longitudinal velocity of the i-th environmental vehicle, and P veh，thres Is the threshold of the vehicle force field;

[0045] Generate the vertical trajectory using a fifth-order polynomial

[0046] d(t) = β0 + β1t + β2t 2 + β3t 3 + β4t 4 + β5t 5

[0047] To solve for the six coefficients, state variables are required That is, the lateral displacement, lateral velocity, and lateral acceleration at the initial moment, and the lateral displacement, lateral velocity, and lateral acceleration at the end moment.

[0048] To generate different trajectories, the constraint from moment m to moment n is defined as

[0049]

[0050] With different d m and time interval T n varying, set Ensure that the last part of the trajectory is in the road direction.

[0051] Furthermore, the generation of the trajectory set in the Frenet coordinate system based on the environmental vehicle information, real-time road information, and real-time ego-vehicle information further includes the selection of the optimal trajectory;

[0052] Combine the longitudinal and lateral trajectory sets and select the optimal trajectory that meets the constraint conditions from the candidate trajectories;

[0053] Define the cost function:

[0054] J tot = w s J s + w d J d + w c J c + w p J p

[0055] Where

[0056]

[0057] which makes the longitudinal movement optimal, c j，s , c v，s , c T，s are the weights for each exponent;

[0058]

[0059] which makes the lateral movement optimal, c j，d , c T，d are the weights for each exponent;

[0060] J c = (d f - d f，opt ) 2

[0061] Considering the consistency of continuous replanning, d f，opt is the previously selected optimal trajectory;

[0062]

[0063] Using the above method, the trajectory result of the ego-vehicle driving towards the center line is completed. The outermost gray part is the restricted invalid trajectory, the middle dark part is the valid trajectory, and the lightest part in the middle is the optimal trajectory selected through the cost function.

[0064] Further, controlling the vehicle to travel according to the optimal trajectory specifically includes:

[0065] After combining the selected lateral optimal trajectory and longitudinal optimal trajectory, convert them into a trajectory in the Cartesian coordinate system for tracking control;

[0066] Select a suitable algorithm to implement the trajectory tracking control.

[0067] A path planning device based on the force field includes: a trajectory calculation unit and a vehicle control unit;

[0068] The trajectory calculation unit is used to establish a risk model, generate a trajectory set in the Frenet coordinate system, and screen out the optimal trajectory from the trajectory set according to the real-time vehicle information, real-time environmental vehicle information, and real-time road information;

[0069] The vehicle control unit is used to control the vehicle to travel according to the optimal trajectory.

[0070] The present invention proposes an intelligent path planning method that combines an adaptive force field model and an optimal trajectory generation method. In order to represent the risk function of a moving object, a fixed headway time strategy is adopted to design the environmental vehicle force field. The proposed intelligent path planning method is applicable to various road autonomous driving situations, such as lane keeping, lane changing, collision avoidance, etc.

[0071] The adaptive force field model proposed by the present invention overcomes the defect of the existing force field model that does not consider the dynamic aspects of obstacles enough by changing the magnitude of the risk. The cost function is optimally designed considering the performance of each part to generate the optimal trajectory.

[0072] The present invention establishes a road autonomous driving force field model to describe the driving risk; generates a trajectory set in the Frenet coordinate system according to the environmental vehicle information, real-time road information, and real-time vehicle information; designs a cost function to select the optimal trajectory, which is used for trajectory tracking after coordinate transformation.

[0073] Other features and advantages of the present invention will be described in the subsequent specification, and part of them will become obvious from the specification or will be understood by implementing the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the structures pointed out in the specification and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0075] Figure 1 This is the flowchart of the method according to the embodiments of the present invention;

[0076] Figure 2 This is the schematic diagram of the unit according to the embodiments of the present invention;

[0077] Figure 3 This is the schematic diagram of the environmental vehicle and the road alignment in the Frenet coordinate system according to the embodiments of the present invention;

[0078] Figure 4 This is the schematic diagram of the safe distance between the host vehicle and the environmental vehicle according to the embodiments of the present invention;

[0079] Figure 5 This is the schematic diagram of the trajectory result according to the embodiments of the present invention. Detailed implementation manners

[0080] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0081] The existing force field model does not consider enough the dynamic aspects of obstacles. The adaptive force field model proposed by the present invention overcomes such defects by changing the magnitude of the risk. The cost function is optimally designed considering the optimal performance of each part to generate an optimal trajectory.

[0082] In a first aspect, as Figure 1 shown, the present invention proposes a path planning method based on a force field, and the method includes:

[0083] Establishing a risk model, generating a trajectory set in the Frenet coordinate system based on real-time host vehicle information, real-time environmental vehicle information, and real-time road information, and screening out the optimal trajectory from the trajectory set;

[0084] Controlling the vehicle to travel according to the optimal trajectory.

[0085] In specific implementation, in order to represent the risk function of a moving object, a fixed headway strategy is adopted to design the force field of the environmental vehicle. The proposed intelligent path planning method is applicable to various road autonomous driving situations, such as lane keeping, lane changing, collision avoidance, etc.

[0086] In this embodiment, the establishing a risk model, generating a trajectory set in the Frenet coordinate system, and screening out the optimal trajectory from the trajectory set includes:

[0087] Establish a road autonomous driving force field model to describe driving risks;

[0088] Generate a trajectory set in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego-vehicle information;

[0089] Design a cost function to select the optimal trajectory, which is used for trajectory tracking after coordinate transformation.

[0090] In this embodiment, the establishment of the road autonomous driving force field model includes an environmental vehicle force field;

[0091] In the Frenet coordinate system, establish an environmental vehicle force field model using a fixed time headway strategy;

[0092] The change in the degree of danger in the s direction (ordinate) is mainly determined by the force field shape coefficient σ vehs , 2σ vehs represents the safe distance between the ego-vehicle and the environmental vehicle in the Frenet coordinate system, and uses the fixed time headway at the current moment Set the reference distance between the ego-vehicle and the environmental vehicle, where D0 and τ are constants greater than 0. Therefore,

[0093] The force field of the i-th environmental vehicle at time k is expressed as:

[0094] k = t + 1,…, t + N p ; where t is the current time, k is the forward prediction time, and the interval between t and k is the prediction horizon N p ;

[0095] where P veh_i represents the force field of the i-th environmental vehicle; A veh represents the maximum value of the vehicle force field; where s and d represent the abscissa and ordinate of the ego-vehicle in the Frenet coordinate system; where s veh_i , d veh_i represent the abscissa and ordinate of the i-th environmental vehicle in the Frenet coordinate system; where c represents the coefficient that determines the shape of the force field of the i-th environmental vehicle; N p represents the prediction horizon.

[0096] In this embodiment, the establishment of the road autonomous driving force field model also includes a road boundary force field, which is expressed as follows:

[0097] m = 1, 2

[0098] where P rb_j represents the force field of the m-th road boundary; A rbRepresents the maximum value of the road boundary force field; y r,rb_m Represents the lateral distance to the m-th road boundary in the geodetic coordinate system; where σ rb Represents the road boundary force field coefficient.

[0099] In this embodiment, establishing the road autonomous driving force field model further includes a lane center line force field, which is expressed as follows:

[0100]

[0101] P ctr_n Represents the force field of the n-th lane center line; A ctr Represents the maximum value of the lane line force field; y r,ctr_n Represents the lateral distance to the n-th lane center line in the geodetic coordinate system; σ ctr Represents the lane center line force field coefficient.

[0102] In this embodiment, generating a trajectory set in the Frenet coordinate system according to the environmental vehicle information, real-time road information, and real-time ego-vehicle information includes calculating the longitudinal speed constraint, trajectory curvature constraint, and longitudinal and lateral acceleration constraints of the trajectory;

[0103] Calculating the longitudinal speed constraint of the trajectory specifically includes:

[0104] Considering the comfort and safety of the driver, the speed in the s direction has a limit, which is expressed as:

[0105]

[0106] Where Represents the maximum lateral acceleration after considering ride comfort; k is the road curvature; Is the speed limit in the s direction

[0107] When the road is straight, the speed limit is very large, and a fixed speed value is given The reference s-direction speed is set as

[0108] Calculating the trajectory curvature constraint specifically includes:

[0109] After the lateral trajectory and the longitudinal trajectory are combined, it is necessary to check the trajectory curvature constraint k cand To mainly consider the vehicle steering limit, k cand ∈[k min , k max , between the maximum and minimum values of the road curvature;

[0110] Calculating the longitudinal and lateral acceleration constraints of the trajectory specifically includes:

[0111] When considering the physical limitations of vehicle dynamics, constraints on longitudinal acceleration and lateral acceleration are required;

[0112]

[0113] where s(t) and d(t) are the ordinate and abscissa of the vehicle at time t in the Frenet coordinate system; a max is the maximum vehicle acceleration.

[0114] In this embodiment, generating a trajectory set in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego-vehicle information further includes generating a candidate path, that is, a trajectory set; the generated trajectory set is decomposed horizontally and longitudinally. The horizontal trajectory is generated using a fourth-order polynomial, and the longitudinal trajectory is generated using a fifth-order polynomial. Finally, the obtained trajectory set is synthesized by combining the horizontal trajectory and the longitudinal trajectory; specifically including:

[0115] Generating a horizontal trajectory using a fourth-order polynomial

[0116] s(t) = α0 + α1t + α2t 2 + α3t 3 + α4t 4

[0117] To solve for the 5 coefficients, state variables are required that is, the longitudinal displacement, longitudinal velocity, and longitudinal acceleration at the initial moment, and the longitudinal velocity and longitudinal acceleration at the end moment;

[0118] To generate different trajectories, the constraint from time m to time n is defined as:

[0119] T n represents the time interval

[0120] When the ego-vehicle and the leading vehicle are in the same lane, if max(P veh_i (k|t)) > P veh，thres

[0121] Then Otherwise,

[0122] is the longitudinal velocity of the i-th environmental vehicle, and P veh，thres is the threshold of the vehicle force field;

[0123] Generating a longitudinal trajectory using a fifth-order polynomial

[0124] d(t) = β0 + β1t + β2t 2 + β3t 3 + β4t 4 + β5t5

[0125] To solve for the six coefficients, the state variables i.e., the lateral displacement, lateral velocity, and lateral acceleration at the initial moment, and the lateral displacement, lateral velocity, and lateral acceleration at the end moment;

[0126] To generate different trajectories, the constraint from time m to time n is defined as

[0127]

[0128] With different d m and the time interval T n changing, let Ensure that the last part of the trajectory is in the road direction.

[0129] In this embodiment, generating a trajectory set in the Frenet coordinate system according to the environmental vehicle information, real-time road information, and real-time ego-vehicle information further includes the selection of the optimal trajectory;

[0130] Combine the longitudinal and lateral trajectory sets and select the optimal trajectory that satisfies the constraint conditions from the candidate trajectories;

[0131] Define the cost function:

[0132] J tot = w s J s + ω d J d + w c J c + w p J p

[0133] Where

[0134]

[0135] which makes the longitudinal motion optimal, c j，s , c v，s , c T，s being the weights for each exponent;

[0136]

[0137] which makes the lateral motion optimal, c j，d， c T,d being the weights for each exponent;

[0138] J c = (d f - d f，opt ) 2

[0139] Considering the consistency of continuous replanning, d f，opt is the optimal trajectory selected previously;

[0140]

[0141] In this embodiment, the controlling the vehicle to travel according to the optimal trajectory specifically includes:

[0142] After combining the selected lateral optimal trajectory and longitudinal optimal trajectory, convert them into a trajectory in the Cartesian coordinate system for tracking control;

[0143] Select a suitable algorithm to implement the trajectory tracking control.

[0144] In a second aspect, as Figure 2 shown, a path planning device based on a force field includes: a trajectory calculation unit and a vehicle control unit;

[0145] The trajectory calculation unit is configured to establish a risk model, generate a trajectory set in the Frenet coordinate system, and screen out the optimal trajectory from the trajectory set according to the real-time vehicle information, real-time environmental vehicle information, and implementation road information;

[0146] The vehicle control unit is configured to control the vehicle to travel according to the optimal trajectory.

[0147] In specific implementation, the implementation processes of a path planning device based on a force field and a path planning method based on a force field of the present invention correspond one by one, and will not be elaborated here.

[0148] To enable those skilled in the art to better understand the present invention, the principle of the present invention is described as follows with reference to the accompanying drawings:

[0149] As Figure 1 shown, the present invention proposes an intelligent path planning method that combines an adaptive force field model and an optimal trajectory generation method. The existing force field model does not consider the dynamic aspects of obstacles enough, and the proposed adaptive force field model overcomes such defects by changing the magnitude of the risk. In order to represent the risk function of a moving object, a fixed headway strategy is adopted. The proposed intelligent path planning method is applicable to various road autonomous driving situations, such as lane keeping, lane changing, collision avoidance, etc.

[0150] 1. Establish a force field model. The road geometric alignment in the frenet coordinate system is as Figure 3 shown,

[0151] In the road autonomous driving environment, the force field includes an environmental vehicle force field, a road boundary force field, and a lane center line force field

[0152] The force field of the i-th environmental vehicle at time k:

[0153] In the Frenet coordinate system, an environmental vehicle force field model is established using a fixed headway strategy.

[0154] The change in the degree of danger in the s-direction (ordinate) is mainly determined by the force field shape coefficient σvehs, 2σ vehs represents the safe distance between the ego vehicle and the environmental vehicle in the Frenet coordinate system, and a fixed headway is adopted Set the reference distance between the ego vehicle and the environmental vehicle, where D0, τ are constants greater than 0. Therefore,

[0155] such as Figure 4 As shown, the schematic of the distance between the ego vehicle and the environmental vehicle in the frenet coordinate system. The force field of the i-th environmental vehicle is expressed as:

[0156]

[0157] k = t + 1,…, t + Np

[0158] Pveh_i represents the force field of the i-th environmental vehicle; A veh represents the maximum value of the vehicle force field; where s, d represent the abscissa and ordinate of the ego vehicle in the Frenet coordinate system; where sveh_i, d veh_i represent the abscissa and ordinate of the i-th environmental vehicle in the Frenet coordinate system; where c represents the coefficient that determines the shape of the force field of the i-th environmental vehicle; N p represents the prediction horizon;

[0159] (2) Road boundary force field:

[0160]

[0161] m = 1, 2

[0162] P rb_j represents the force field of the m-th road boundary; A rb represents the maximum value of the road boundary force field; yr, rb_m represents the lateral distance to the m-th road boundary in the earth coordinate system; σrb represents the road boundary force field coefficient;

[0163] Lane centerline force field:

[0164]

[0165] Pctr_n represents the force field of the n-th lane centerline; Actr represents the maximum value of the lane line force field; yr, ctr_n represents the lateral distance to the n-th lane centerline in the earth coordinate system; σctr represents the lane centerline force field coefficient;

[0166] 2. Generate the optimal trajectory

[0167] When generating a polynomial trajectory based on sampling, the vehicle motion is generally not considered. Therefore, vehicle kinematic and dynamic constraints need to be considered, and road rules also need to be taken into account. Since the lateral trajectory and the longitudinal trajectory are generated separately in the Frenet coordinate system, many constraints need to be checked in the Frenet coordinate system.

[0168] 2.1 Constraints

[0169] (1) Longitudinal velocity constraint of the trajectory

[0170] Considering the comfort and safety of the driver, the speed in the s direction has a limit, which is expressed as:

[0171]

[0172] represents the maximum lateral acceleration after considering ride comfort; k is the road curvature; is the speed limit in the s direction

[0173] When the road is straight, the speed limit is very large, and a fixed speed value is given

[0174] Therefore, the reference s-direction speed is set to

[0175] (2) Trajectory curvature constraint

[0176] After the lateral trajectory and the longitudinal trajectory are combined, the trajectory curvature constraint k cand needs to be checked, mainly considering the vehicle steering limit, k cand ∈ [k min , k max , between the maximum and minimum values of the road curvature.

[0177] (3) Longitudinal and lateral acceleration constraints of the trajectory

[0178] When considering the physical limitations of vehicle dynamics, the longitudinal acceleration and the lateral acceleration need to be constrained.

[0179]

[0180] a max is the maximum vehicle acceleration.

[0181] 2.2 Generate candidate paths

[0182] The following is used to generate the lateral trajectory using a fourth-order polynomial:

[0183] s(t) = α0 + α1t + α2t 2 + α3t 3 + α4t 4 ;

[0184] To solve for the 5 coefficients, the state variables are the longitudinal displacement, longitudinal velocity, longitudinal acceleration at the initial moment, and the longitudinal velocity and longitudinal acceleration at the end moment. To generate different trajectories, the constraints from moment m to moment n are defined as follows:

[0185] T n represents the time interval.

[0186] When the ego vehicle and the leading vehicle are in the same lane, if max(P veh_i (k|t)) > P veh，thres

[0187] Then Otherwise,

[0188] is the longitudinal velocity of the i-th environmental vehicle, and P veh，thres is the threshold of the vehicle force field.

[0189] The longitudinal trajectory is generated using a fifth-order polynomial as follows:

[0190] d(t) = β0 + β1t + β2t 2 + β3t 3 + β4t 4 + β5t 5

[0191] To solve for the six coefficients, the state variables are the lateral displacement, lateral velocity, lateral acceleration at the initial moment, and the lateral displacement, lateral velocity, lateral acceleration at the end moment.

[0192] To generate different trajectories, the constraints from moment m to moment n are defined as

[0193]

[0194] As different d m and the time interval T n vary, let Ensure that the last part of the trajectory is in the road direction.

[0195] 2.3 Selection of the Optimal Trajectory

[0196] Combine the longitudinal and lateral trajectory sets and select the optimal trajectory that satisfies the constraint conditions from the candidate trajectories.

[0197] Define the cost function:

[0198] J tot = w s J s + w d J d + w c J c + w p J p

[0199] J tot This formula consists of four terms, considering four control objectives. The first term, that is, the first control objective, is related to J s To ensure optimal longitudinal movement, the second term, that is, the second control objective, is related to J d To ensure optimal lateral movement, the third term, that is, the third control objective, is related to J c To ensure the consistency of continuous replanning, the fourth term, that is, the fourth control objective, is related to J p It is related to the previous force field and field strength, to ensure safety.

[0200] Among them,

[0201]

[0202] It makes the longitudinal movement optimal, c j，s c v，s c T，s are the weights of each exponent;

[0203]

[0204] It makes the lateral movement optimal, c j，d c T，d are the weights of each exponent;

[0205] J c = (d f - d f，opt ) 2

[0206] Considering the consistency of continuous replanning, d f，opt is the previously selected optimal trajectory;

[0207]

[0208] Using the above method, the trajectory result of the vehicle driving towards the center line is completed. The planned trajectory towards the center line is as shown in Figure 5 The outermost gray part is the restricted invalid trajectory, the middle dark part is the valid trajectory, and the lightest part in the middle is the optimal trajectory selected by the cost function.

[0209] 3. Trajectory Tracking Control

[0210] After combining the selected horizontal optimal trajectory and vertical optimal trajectory in the previous part, it is converted into a trajectory in the Cartesian coordinate system for tracking control. Appropriate algorithms such as the pure tracking method, Stanley method, etc. can be selected to implement the trajectory tracking control.

[0211] Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method based on a force field, characterized in that, The method includes: Based on real-time ego vehicle information, real-time environmental vehicle information, and real-time road information, establish a risk model in the Frenet coordinate system, generate a set of trajectories, and select the optimal trajectory from the set of trajectories; Control the vehicle to move forward according to the optimal trajectory; The establishment of the risk model in the Frenet coordinate system, generation of the set of trajectories, and selection of the optimal trajectory from the set of trajectories include: Establish a road autonomous driving force field model to describe driving risks; Generate a set of trajectories in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego vehicle information; Design a cost function to select the optimal trajectory, which is used for trajectory tracking after coordinate transformation; The establishment of the road autonomous driving force field model includes an environmental vehicle force field; In the Frenet coordinate system, establish an environmental vehicle force field model using the fixed headway strategy; The s direction, that is, the change in the degree of danger on the vertical coordinate is mainly determined by the shape coefficient of the force field σ vehs , 2σ vehs represents the safe distance between the host vehicle and the surrounding vehicle in the Frenet coordinate system, and the fixed headway at the current moment is adopted Set the reference distance between the host vehicle and the surrounding vehicle, where D0 and τ are constants greater than 0. Therefore The force field of the i-th environmental vehicle at time k is expressed as: k = t + 1, …, t + N p where t is the current time, k is the forward prediction time, and the interval between t and k is the prediction time domain N p ; Among which P veh_i represents the influence field of the i-th environmental vehicle; A veh represents the maximum value of the vehicle influence field; where s and d represent the abscissa and ordinate of the ego vehicle in the Frenet coordinate system; where s veh_i , d veh_i represent the abscissa and ordinate of the i-th environmental vehicle in the Frenet coordinate system; where c represents the coefficient determining the shape of the influence field of the i-th environmental vehicle; N p represents the prediction horizon.

2. The path planning method based on a force field according to claim 1, characterized in that, The establishment of the road autonomous driving force field model also includes a road boundary force field, which is expressed as follows: Among which P rb_j represents the force field of the m-th road boundary; A rb represents the maximum value of the road boundary force field; y r,rb_m represents the lateral distance to the m-th road boundary in the geodetic coordinate system; among which σ rb represents the road boundary force field coefficient.

3. The path planning method based on a force field according to claim 1, characterized in that, The establishment of the road autonomous driving force field model also includes a lane centerline force field, which is expressed as follows: P ctr_n represents the force field of the center line of the nth lane; A ctr represents the maximum value of the lane line force field; y r,ctr_n represents the lateral distance from the center line of the nth lane in the geodetic coordinate system; σ ctr represents the lane center line force field coefficient.

4. The path planning method based on a force field according to claim 1, characterized in that, The generation of the set of trajectories in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego vehicle information includes calculating the longitudinal velocity constraint of the trajectory, the curvature constraint of the trajectory, and the longitudinal and lateral acceleration constraints of the trajectory; The calculation of the longitudinal velocity constraint of the trajectory specifically includes: Considering the comfort and safety of the driver, the speed in the s direction is limited, which is expressed as: Among them represents the maximum lateral acceleration after considering ride comfort; k is the road curvature; is the speed limit in the s direction When the road is straight, the speed limit is very high, and a fixed speed value is given The reference s-direction speed is set to The calculation of the curvature constraint of the trajectory specifically includes: After the horizontal trajectory and the vertical trajectory are merged, it is necessary to check the trajectory curvature constraint k cand to consider mainly the vehicle steering limit, k cand ∈[k min , k max , which is between the maximum and minimum values of the road curvature; The calculation of the longitudinal and lateral acceleration constraints of the trajectory specifically includes: When considering the physical limitations of vehicle dynamics, it is necessary to constrain the longitudinal acceleration and the lateral acceleration; where s(t) and d(t) are the ordinate and abscissa of the vehicle at time t in the Frenet coordinate system; a max is the maximum vehicle acceleration.

5. The path planning method based on the force field according to claim 1, characterized in that, The generation of the set of trajectories in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego vehicle information also includes generating candidate paths, that is, the set of trajectories; the generated set of trajectories is decomposed horizontally and longitudinally. The horizontal trajectory is generated using a fourth-order polynomial, and the longitudinal trajectory is generated using a fifth-order polynomial. Finally, the obtained set of trajectories needs to synthesize the horizontal trajectory and the longitudinal trajectory; specifically includes: Generate the horizontal trajectory using a fourth-order polynomial s(t) = α0 + α1t + α2t 2 + α3t 3 + α4t 4 To solve for the five coefficients, state variables are required That is, the longitudinal displacement, longitudinal velocity, and longitudinal acceleration at the initial moment, and the longitudinal velocity and longitudinal acceleration at the end moment; To generate different trajectories, the constraint from the m-th moment to the n-th moment is defined as: T n Indicates a time interval When the host vehicle and the leading vehicle are in the same lane, if max(P veh_i (k|t)) > P veh,thres Then Otherwise is the longitudinal speed of the i-th environmental vehicle, P veh,thres is the threshold of the vehicle force field; Generate the longitudinal trajectory using a fifth-order polynomial d(t) = β0 + β1t + β2t 2 + β3t 3 + β4t 4 + β5t 5 To solve for the six coefficients, the state variables i.e., the lateral displacement, lateral velocity, and lateral acceleration at the initial moment, and the lateral displacement, lateral velocity, and lateral acceleration at the end moment; To generate different trajectories, define the constraint from time m to time n as As different d m and time interval T n vary, set to ensure that the last part of the trajectory is in the road direction.

6. The path planning method based on the force field according to claim 1, characterized in that, The generation of the set of trajectories in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego vehicle information also includes the selection of the optimal trajectory; Combine the longitudinal and horizontal trajectory sets and select the optimal trajectory that meets the constraint conditions from the candidate trajectories; Define the cost function: J tot = w s J s + w d J d + w c J c + w p J p where It optimizes the longitudinal movement, c j，s , c b，s , c T，s is the weight for each exponent; It optimizes the lateral movement, c j，d , c T，d is the weight for each exponent; J c = (d f - d f,opt ) 2 Considering the consistency of continuous replanning, d f，opt is the optimal trajectory selected previously; 7. The path planning method based on the force field according to claim 1, characterized in that, The control of the vehicle to move forward according to the optimal trajectory specifically includes: After combining the selected horizontal optimal trajectory and the longitudinal optimal trajectory, convert them into a trajectory in the Cartesian coordinate system for tracking control; Select a suitable algorithm to implement the trajectory tracking control.

8. A path planning device based on the force field, characterized in that, It includes: A trajectory calculation unit and a vehicle control unit; The trajectory calculation unit is used to establish a risk model in the Frenet coordinate system, generate a set of trajectories, and select the optimal trajectory from the set of trajectories based on real-time ego vehicle information, real-time environmental vehicle information, and real-time road information; The vehicle control unit is used to control the vehicle to move forward according to the optimal trajectory; Establishing a risk model, generating a trajectory set in the Frenet coordinate system, and screening out the optimal trajectory from the trajectory set includes: Establishing a road autonomous driving force field model to describe driving risks; Generating a trajectory set in the Frenet coordinate system according to environmental vehicle information, real-time road information, and real-time ego-vehicle information; Designing a cost function to select the optimal trajectory, which is used for trajectory tracking after coordinate transformation; The establishment of the road autonomous driving force field model includes the environmental vehicle force field; In the Frenet coordinate system, a fixed headway strategy is adopted to establish an environmental vehicle force field model; The s direction, that is, the change in the degree of danger on the vertical coordinate is mainly determined by the shape coefficient of the force field σ vehs , 2σ vehs represents the safe distance between the ego vehicle and the surrounding vehicle in the Frenet coordinate system, and the fixed headway at the current moment is adopted Set the reference distance between the ego vehicle and the surrounding vehicle, where D0 and τ are constants greater than 0. Therefore,[[]] The force field of the i-th environmental vehicle at time k is expressed as: k = t + 1, …, t + N p where t is the current time, k is the forward prediction time, and the interval between t and k is the prediction time domain N p ; Among which P veh_i represents the influence field of the i-th environmental vehicle; A veh represents the maximum value of the vehicle influence field; where s and d represent the abscissa and ordinate of the ego vehicle in the Frenet coordinate system; where s veh_i , d veh_i represent the abscissa and ordinate of the i-th environmental vehicle in the Frenet coordinate system; where c represents the coefficient determining the shape of the influence field of the i-th environmental vehicle; N p represents the prediction horizon.

Images