Vehicle trajectory planning method and device, storage medium and electronic equipment

Abstract

The present disclosure relates to the field of automatic driving, and particularly relates to a vehicle trajectory planning method, a vehicle trajectory planning device, a storage medium and an electronic device. The vehicle trajectory planning method comprises taking a current position of a vehicle as a starting point of a planning trajectory curve; performing space-time grid sampling based on the starting point to obtain a plurality of ending points of the planning trajectory curve, so as to obtain a plurality of planning trajectory curves according to the starting point and the plurality of ending points; performing evaluation on the plurality of planning trajectory curves according to a planning trajectory cost function to obtain a trajectory evaluation result; the planning trajectory cost function comprises a first cost function constructed based on a forward prediction result of a driver target trajectory in a co-driving system of the vehicle and a second cost function constructed based on a backward estimation result of the driver target trajectory; and determining a target planning trajectory curve from the plurality of planning trajectory curves based on the evaluation result. The method provided by the present disclosure can solve the problem of rapid synchronization of human-machine target trajectories under the condition of decision conflict in a human-machine co-driving system.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the field of autonomous driving, and in particular, to a vehicle trajectory planning method, a vehicle trajectory planning device, a storage medium and an electronic device. BACKGROUND

[0002] In the field of autonomous driving technology, an intelligent driving system should give the driver more decision-making freedom, such as lane changing, overtaking, and merging, to cope with complex and diverse driving tasks in actual driving scenarios.

[0003] When both human and machine decision-making systems exist in a co-driving system, accurate and early estimation of the driver's decision-making becomes the primary problem that needs to be solved in co-driving system decision-making research. However, in actual co-driving systems, when the driver's steering torque is disturbed by the driving assistance system, and when the human-machine steering conflict is large, the driver's intention or target trajectory is difficult to accurately predict through the constant desired yaw rate model.

[0004] It should be noted that the information disclosed in the above background section is only used to strengthen the understanding of the background of the present disclosure, and therefore can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0005] The purpose of the present disclosure is to provide a vehicle trajectory planning method, a vehicle trajectory planning device, a storage medium and an electronic device, aiming to solve the problem of rapid synchronization of human-machine target trajectories in the condition of decision-making conflict in vehicle trajectory planning in a human-machine co-driving system.

[0006] Other characteristics and advantages of the present disclosure will become apparent from the following detailed description, or will be learned by practice of the present disclosure.

[0007] According to an aspect of an embodiment of the present disclosure, a vehicle trajectory planning method is provided, comprising: taking the current position of a vehicle as the starting point of a planning trajectory curve; performing space-time grid sampling based on the starting point to obtain a plurality of end points of the planning trajectory curve, so as to obtain a plurality of planning trajectory curves according to the starting point and the plurality of end points; evaluating the plurality of planning trajectory curves according to a planning trajectory cost function to obtain a trajectory evaluation result; wherein the planning trajectory cost function comprises a first cost function constructed based on a forward prediction result of a driver target trajectory in a co-driving system of the vehicle and a second cost function constructed based on a backward estimation result of the driver target trajectory; and determining a target planning trajectory curve from the plurality of planning trajectory curves based on the evaluation result.

[0008] According to some embodiments of the present disclosure, based on the foregoing scheme, the obtaining the plurality of end points of the plurality of planning trajectory curves based on the start point comprises: constructing a first coordinate system based on the shape of the road where the vehicle is located; sampling the start point along a first coordinate axis of the first coordinate system at equal intervals p times to obtain Np mileage points; wherein p is a positive integer; sampling each of the mileage points along a second coordinate axis of the first coordinate system at equal intervals q times to obtain Np*Nq end points of the plurality of planning trajectory curves; wherein q is a positive integer.

[0009] According to some embodiments of the present disclosure, based on the foregoing scheme, the obtaining the plurality of planning trajectory curves based on the start point and the plurality of end points comprises: determining a grid start point state vector of the start point and a grid end point state vector of each of the end points in the first coordinate system; and fitting the start point and each of the end points based on the grid start point state vector and the grid end point state vector to obtain the plurality of planning trajectory curves.

[0010] According to some embodiments of the present disclosure, based on the foregoing scheme, the method further comprises: determining the grid start point state vector, wherein the determining the grid start point state vector comprises: extracting map information of a location where the vehicle is located; obtaining measurement data based on the map information; and determining the grid start point state vector based on the measurement data. According to some embodiments of the present disclosure, based on the foregoing scheme,

[0011] According to some embodiments of the present disclosure, based on the foregoing scheme, the determining the grid start point state vector based on the measurement data comprises: extracting a lateral mileage and a lateral displacement of the start point in the first coordinate system based on the measurement data; determining a heading angle of the vehicle in the first coordinate system based on a heading angle of the start point in the second coordinate system and a heading angle of the lane center line in the measurement data; calculating a yaw rate of the start point in the first coordinate system based on a vehicle speed of the vehicle, a yaw rate in the second coordinate system, a curvature of the lane center line, and the lateral displacement in the first coordinate system in the measurement data; and determining the grid start point state vector based on the lateral mileage, the lateral displacement, the heading angle, and the yaw rate of the start point in the first coordinate system.

[0012] According to some embodiments of the present disclosure, based on the foregoing scheme, the method further comprises: determining the grid end point state vector, wherein the determining the grid end point state vector comprises: determining a lateral mileage and a lateral displacement of the end point in the first coordinate system based on the result of the spatiotemporal grid sampling; configuring a heading angle and a yaw rate of the end point in the first coordinate system; and determining the grid end point state vector based on the lateral mileage, the lateral displacement, the heading angle, and the yaw rate of the end point in the first coordinate system.

[0013] According to some embodiments of the present disclosure, based on the foregoing scheme, the method further comprises: constructing the planning trajectory cost function, the constructing the planning trajectory cost function comprising: constructing a first cost function based on an error between the planning trajectory curve and a forward prediction result of the driver target trajectory in the co-driving system; constructing a second cost function based on an error between the planning trajectory curve and a backward estimation result of the driver target trajectory in the co-driving system; respectively configuring a first weight of the first cost function and a second weight of the second cost function; and constructing the planning trajectory cost function according to the first cost function, the second cost function, the first weight and the second weight.

[0014] According to some embodiments of the present disclosure, based on the foregoing scheme, the respectively configuring a first weight of the first cost function and a second weight of the second cost function comprises: calculating a driver steering power and an automatic driving system steering power in the co-driving system to obtain a human-machine steering power difference; and determining the first weight and the second weight based on the human-machine steering power difference.

[0015] According to some embodiments of the present disclosure, based on the foregoing scheme, the constructing the planning trajectory cost function according to the first cost function, the second cost function, the first weight and the second weight comprises: constructing a third cost function based on a lane keeping property of the planning trajectory curve; constructing a fourth cost function based on a lateral acceleration of the planning trajectory curve; constructing a fifth cost function based on a lateral jerk of the planning trajectory curve; respectively configuring a third weight of the third cost function, a fourth weight of the fourth cost function and a fifth weight of the fifth cost function; and constructing the planning trajectory cost function according to the first cost function, the second cost function, the third cost function, the fourth cost function, the fifth cost function, the first weight, the second weight, the third weight, the fourth weight and the fifth weight.

[0016] According to some embodiments of the present disclosure, based on the foregoing scheme, the method further comprises: determining a backward estimation result of the driver target trajectory in the co-driving system, the determining the backward estimation result of the driver target trajectory in the co-driving system comprising: constructing a driver trajectory tracking control objective function with a minimum error between the planning trajectory curve and the driver target trajectory, and constructing a system trajectory tracking control objective function with a minimum error between the planning trajectory curve and the automatic driving system target trajectory; and obtaining the backward estimation result of the driver target trajectory based on a Nash equilibrium strategy of the distributed model predictive control for solving the driver trajectory tracking control objective function and the automatic driving system trajectory tracking control objective function.

[0017] According to some embodiments of the present disclosure, based on the foregoing scheme, the constructing the driver trajectory tracking control objective function with minimum error between the planned trajectory curve and the driver target trajectory and the constructing the system trajectory tracking control objective function with minimum error between the planned trajectory curve and the automatic driving system comprises: calculating a predicted lateral displacement of the vehicle; determining a driver preview vehicle lateral displacement at a preset preview distance within a driver control time domain, and constructing the driver trajectory tracking control objective function based on the predicted lateral displacement and the driver preview vehicle lateral displacement; and determining a system preview vehicle lateral displacement at the preset preview distance within an automatic driving system control time domain, and constructing the system trajectory tracking control objective function based on the predicted lateral displacement and the system preview vehicle lateral displacement.

[0018] According to some embodiments of the present disclosure, based on the foregoing scheme, the solving the driver trajectory tracking control objective function and the automatic driving system trajectory tracking control objective function based on the distributed model predictive control to obtain the backward estimation result of the driver target trajectory comprises: obtaining a driver control strategy expression and an automatic driving system control strategy expression based on the driver trajectory tracking control objective function and the automatic driving system trajectory tracking control objective function; solving the driver control strategy expression and the automatic driving system control strategy expression by using a convex iteration algorithm to obtain a decoupled human-machine open-loop Nash equilibrium solution; obtaining a lateral displacement expression at a driver preview point according to the human-machine open-loop Nash equilibrium solution; and obtaining the backward estimation result of the driver target trajectory by solving a least squares solution of the lateral displacement expression by using a pseudo-inverse method.

[0019] According to a second aspect of the embodiments of the present disclosure, a vehicle trajectory planning device is provided, comprising: an acquisition module configured to acquire a current position of a vehicle as a starting point of a planned trajectory curve; a sampling module configured to perform space-time grid sampling based on the starting point to obtain a plurality of ending points of the planned trajectory curve, so as to obtain a plurality of planned trajectory curves according to the starting point and the plurality of ending points; an evaluation module configured to evaluate the plurality of planned trajectory curves according to a planned trajectory cost function to obtain trajectory evaluation results; wherein the planned trajectory cost function comprises a first cost function constructed based on a forward prediction result of a driver target trajectory in a co-driving system of the vehicle and a second cost function constructed based on a backward estimation result of the driver target trajectory; and a determination module configured to determine a target planned trajectory curve from the plurality of planned trajectory curves based on the evaluation results.

[0020] According to a third aspect of the embodiments of the present disclosure, a computer readable storage medium is provided, which stores a computer program, and the program is executed by a processor to implement the vehicle trajectory planning method in the above embodiments.

[0021] According to a fourth aspect of the embodiments of the present disclosure, an electronic device is provided, and has characteristics comprising: one or more processors; a storage device configured to store one or more programs, which, when executed by the one or more processors, cause the one or more processors to implement the vehicle trajectory planning method in the above embodiments.

[0022] The example embodiments of the present disclosure can have the following partial or all beneficial effects:

[0023] In the technical solutions provided in some embodiments of the present disclosure, the human-machine collaborative vehicle trajectory planning method based on the space-time grid method obtains a plurality of planning trajectory curves, and the forward prediction result and the backward estimation result of the driver target trajectory in the co-driving system are simultaneously added to the planning trajectory cost function for evaluation of the planning trajectory curve in the co-driving system, and finally the optimal target planning trajectory curve is determined as the result of vehicle trajectory planning. Based on the above method, the human-machine decision fusion considering the forward prediction result and the backward estimation result of the driver target trajectory during trajectory planning can reduce the deviation between the planning trajectory of the automatic driving system and the driver target trajectory, on the one hand, the decision freedom of the driver is improved, and on the other hand, the decision difference between the driver and the automatic driving system in the co-driving system is reduced.

[0024] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and cannot limit the present disclosure. BRIEF DESCRIPTION OF DRAWINGS

[0025] The drawings herein are incorporated into the specification and form a part of the specification, show embodiments consistent with the present disclosure, and together with the specification serve to explain the principles of the present disclosure. It is obvious that the drawings in the following description are only some embodiments of the present disclosure, and other drawings can be obtained from these drawings without creative labor for those skilled in the art. In the drawings:

[0026] Figure 1 The flowchart schematically shows a vehicle trajectory planning method in an example embodiment of the present disclosure;

[0027] Figure 2 The schematic diagram schematically shows two coordinate systems in an example embodiment of the present disclosure;

[0028] Figure 3 The schematic diagram schematically shows a planning trajectory curve in a Frenet coordinate system in an example embodiment of the present disclosure;

[0029] Figure 4 The flowchart schematically shows a method for determining a grid starting point state vector in an example embodiment of the present disclosure;

[0030] Figure 5 Fig. 1 schematically shows a flowchart of a method for determining a grid end state vector according to an example embodiment of the present disclosure;

[0031] Figure 6 Fig. 2 schematically shows a planning trajectory curve according to an example embodiment of the present disclosure;

[0032] Figure 7 Fig. 3 schematically shows a flowchart of a method for constructing a planning trajectory cost function according to an example embodiment of the present disclosure;

[0033] Figure 8 Fig. 4 schematically shows a relationship between a first weight and a second weight according to an example embodiment of the present disclosure;

[0034] Figure 9 Fig. 5 schematically shows a flowchart of a method for determining a backward estimation result according to an example embodiment of the present disclosure;

[0035] Figure 10 Fig. 6 schematically shows a composition of a vehicle trajectory planning device according to an example embodiment of the present disclosure;

[0036] Figure 11 Fig. 7 schematically shows a computer readable storage medium according to an example embodiment of the present disclosure;

[0037] Figure 12 Fig. 8 schematically shows a computer system of an electronic device according to an example embodiment of the present disclosure. DETAILED DESCRIPTION

[0038] Example implementations will now be described more fully with reference to the accompanying drawings. Example implementations can be implemented in any

[0039] Moreover, described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a thorough understanding of embodiments of the disclosure. One skilled in the relevant art will recognize, however, that the techniques described can be practiced without one or more of the specific details, or with other methods, components, materials, and so forth. In other instances, well-known structures, devices, implementations, or operations are not shown or described in detail to avoid obscuring aspects of the disclosure.

[0040] The block diagrams shown in the drawings are merely functional entities and do not necessarily have to correspond to physically independent entities. That is, the functional entities can be implemented in the form of software, or in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0041] The flowcharts shown in the drawings are merely exemplary illustrations and do not necessarily include all contents and operations / steps, nor are they necessarily executed in the order described. For example, some operations / steps can be further decomposed, and some operations / steps can be combined or partially combined, so the actual execution order can be changed according to actual conditions.

[0042] Since the research on human-machine co-driving cannot be limited to this single driving mode of human-machine collaborative lane keeping, the intelligent driving system should give the driver more freedom of decision-making (such as lane changing, overtaking, and lane merging) in actual driving to cope with complex and diverse driving tasks in actual driving scenarios. To make the co-driving system adapt to more complex driving scenarios, it is necessary to study the human-machine decision-making interaction.

[0043] When both human and machine decision-making systems exist in the co-driving system, decision-making differences also become one of the direct causes of human-machine control conflicts, and it is necessary to further improve the human-machine coordination of the co-driving system through the study of human-machine collaborative decision-making. On the one hand, in the co-driving system, the real-time intention of the driver provides important information for the automatic driving system to predict the driver's behavior, assess the driving risk of the vehicle, and understand complex driving scenarios. Misjudgment or neglect of the driver's true intention will lead to serious human-machine conflicts, thereby increasing the additional driving burden and reducing the driver's reliance on the driving assistance system, and even threatening the safety of vehicle driving. On the other hand, the driver's decision-making is not transparent and is difficult to be directly measured by sensors, so accurate and early estimation of the driver's decision-making becomes the primary problem that needs to be solved in the decision-making research of the co-driving system.

[0044] The above identification method mainly applies the steering system and vehicle state information to qualitatively classify the driver's behavior. For the human-machine co-driving system, the quantitative early prediction of the driver's intention is the basis for the trajectory planning of the automatic driving system. At the same time, the steering system and the vehicle state are often the result of human-machine interaction coupling, which brings difficulties to the identification of the driver's intention.

[0045] Therefore, the present application focuses on the accurate, quantitative and early identification of the lane-changing intention of the driver in the co-driving system. In the prior art, Mohan et al. of the University of California, San Diego found that the eye movement and head movement behavior of the driver also showed significant characteristics before the lane-changing decision, and the behavior specificity appeared earlier than the steering behavior and the vehicle motion state of the driver, so that the early estimation of the driver's decision can be realized. At the same time, the eye movement and head movement behavior of the driver are not coupled with the automatic driving system, and it is an important method to estimate the driver's intention in the co-driving system. Since the deep learning method based on the recurrent neural network (RNN) has been proved to be the most effective means to solve the time sequence problem such as natural language processing (NLP), compared with the above-mentioned classical pattern recognition method, the deep learning method has stronger model expression and semantic segmentation ability of information, and it is proved that the RNN model has significant advantages in the classification performance of the driver's intention compared with the classical machine learning model. In addition to RNN, the convolutional neural network (CNN) which has made a great contribution in the field of image recognition also shows good performance in processing time sequence problems, and in some cases its recognition effect is even stronger than the RNN model.

[0046] After the forward prediction of the driver's decision is realized by the driver's intention recognition method, the intelligent driving system can perform safety evaluation and trajectory planning based on the driver's decision. When the driver's intention is within the driving safety envelope, the system plans a path consistent with the driver to assist the driver in completing the decision goal; when the driver's decision does not meet the safety envelope, the intelligent driving system intervenes in the driver's manipulation through kinematic and dynamic constraint trajectory planning and dynamic allocation of control rights to ensure safe driving of the car.

[0047] In the co-driving system, since the driver's steering torque is disturbed by the driving assistance system, when the human-machine steering conflict is large, the driver's intention or target trajectory is difficult to accurately predict through the constant desired yaw rate model, and the above-mentioned trajectory planning research has not touched the resolution problem of human-machine decision divergence. In the human-machine collaborative decision-making layer of the co-driving system, in addition to considering the forward prediction result of the driver's intention, the trajectory planning algorithm also evaluates and corrects the trajectory planned by the automatic driving system through the human-machine decision conflict, which is also a key method to avoid human-machine decision divergence and realize human-machine decision synchronization.

[0048] Therefore, although the forward prediction result of the driver target trajectory in the prior art makes an early and long-term estimation on the lane changing decision of the driver, since the accuracy rate of the model in estimating the decision of the driver is difficult to reach 100%, a feedback correction algorithm is still needed to correct the forward prediction result of the driver target trajectory through the backward estimation result.

[0049] In summary, in view of the defects in the prior art and considering that the forward prediction error of the driver target trajectory will lead to a disagreement between the human and machine decisions and further cause a steering torque conflict, the present disclosure first proposes a backward estimation algorithm of the driver target trajectory based on a non-cooperative game theory, corrects the estimated value of the driver target trajectory under the disagreement between the human and machine decisions according to the human and machine steering torque conflict, so as to ensure the rapid synchronization of the human and machine decisions. Then, a human and machine collaborative trajectory planning method based on a space-time grid method is proposed, which innovatively adds the forward prediction result of the driver target trajectory based on the eye movement information of the driver and the backward estimation result of the driver target trajectory based on the human and machine steering conflict into the cost function of the trajectory planning of the automatic driving system, so as to realize a human and machine decision fusion method considering the eye movement behavior of the driver and the human and machine steering conflict.

[0050] The implementation details of the technical solutions of the embodiments of the present disclosure are described in detail below.

[0051] Figure 1 A flowchart schematically showing a vehicle trajectory planning method in an exemplary embodiment of the present disclosure is shown. As shown in the figure, Figure 1 The vehicle trajectory planning method includes steps S101 to S104:

[0052] Step S101, taking the current position of the vehicle as the starting point of the planned trajectory curve;

[0053] Step S102, performing space-time grid sampling based on the starting point to obtain a plurality of ending points of the planned trajectory curve, so as to obtain a plurality of planned trajectory curves according to the starting point and the plurality of ending points;

[0054] Step S103, evaluating the plurality of planned trajectory curves according to a planned trajectory cost function to obtain a trajectory evaluation result; wherein the planned trajectory cost function includes a first cost function constructed based on the forward prediction result of the driver target trajectory in the co-driving system of the vehicle and a second cost function constructed based on the backward estimation result of the driver target trajectory;

[0055] Step S104, determining a target planned trajectory curve from the plurality of planned trajectory curves based on the evaluation result.

[0056] In the technical solution provided in some embodiments of the present disclosure, the human-machine collaborative vehicle trajectory planning method based on the space-time grid method obtains multiple planning trajectory curves, and the forward prediction result and the backward estimation result of the driver target trajectory in the co-driving system are simultaneously added to the planning trajectory cost function for evaluation of the planning trajectory curve in the co-driving system, and finally the optimal target planning trajectory curve is determined as the result of vehicle trajectory planning. Based on the above method, the human-machine decision fusion considering the forward prediction result and the backward estimation result of the driver target trajectory during trajectory planning can reduce the deviation between the planning trajectory of the automatic driving system and the driver target trajectory, which on the one hand improves the decision freedom of the driver, and on the other hand reduces the decision divergence between the driver and the automatic driving system in the co-driving system.

[0057] In the following, the steps of the vehicle trajectory planning method in the present example embodiment will be described in more detail in combination with the accompanying drawings and examples.

[0058] In step S101, the current position of the vehicle is taken as the starting point of the planning trajectory curve.

[0059] In one embodiment of the present disclosure, the space-time grid method is used for vehicle trajectory planning. The space-time grid method belongs to the sampling-based trajectory planning method, which takes the current position of the vehicle as the starting point, samples the space-time grid according to the road shape to obtain multiple grid points in the grid as the end point, and then plans the vehicle trajectory.

[0060] It should be noted that the current position of the vehicle can be described with reference to two coordinate systems. Figure 2 The schematic diagram of the two coordinate systems in the exemplary embodiment of the present disclosure is shown schematically, referring to Figure 2 As shown, the first coordinate system and the second coordinate system are included.

[0061] The first coordinate system is the Frenet (road) coordinate system, which is constructed according to the shape of the road. The Frenet coordinate system is composed of s-axis and l-axis, the s-axis extends along the road center line, representing the road mileage, and the l-axis extends along the normal direction of the road center line, representing the lateral deviation of the vehicle from the lane center line. At the same time, a plurality of equidistant s-axes and l-axes form a space-time grid, which includes a plurality of grid points.

[0062] The second coordinate system is the global coordinate system, which is the coordinate system of the vehicle in the two-dimensional space and does not change with the road where the vehicle is located. The global coordinate system is composed of x-axis and y-axis, the x-axis extends in the transverse direction, used to represent the longitudinal displacement of the vehicle, and the y-axis extends in the longitudinal direction, used to represent the lateral displacement of the vehicle.

[0063] In step S102, multiple end points of the planning trajectory curve are obtained based on the space-time grid sampling of the starting point, so as to obtain multiple planning trajectory curves according to the starting point and the multiple end points.

[0064] In one embodiment of the present disclosure, the spatiotemporal grid sampling is first performed based on the starting point, that is, the starting point is grid sampled according to the shape of the road where the vehicle is located to obtain a series of grid points as the endpoints of the planned trajectory curves.

[0065] Then, connecting the starting point and the endpoints can obtain a planned trajectory curve. Since the grid sampling obtains a plurality of endpoints, the same number of planned trajectory curves as the endpoints can be obtained.

[0066] Further, in one embodiment of the present disclosure, the step S102 of performing the spatiotemporal grid sampling based on the starting point to obtain the endpoints of the plurality of planned trajectory curves comprises:

[0067] Step one, constructing a first coordinate system based on the shape of the road where the vehicle is located;

[0068] Step two, sampling the starting point along a first coordinate axis of the first coordinate system at equal intervals p times to obtain Npmileage points; wherein p is a positive integer;

[0069] Step three, sampling each of the mileage points along a second coordinate axis of the first coordinate system at equal intervals q times to obtain Np*Nqendpoints of the planned trajectory curves; wherein q is a positive integer.

[0070] Specifically, when performing the spatiotemporal grid sampling, it is necessary to perform grid sampling in the direction of travel according to the shape of the road where the vehicle is located. Therefore, in step one, in order to facilitate sampling, it is necessary to first construct a Frenet coordinate system according to the shape of the road, that is, to take the center line of the road where the vehicle is located as the s axis and the normal direction of the center line of the road as the l axis.

[0071] In steps two and three, after the Frenet coordinate system is constructed, grid sampling points are sampled at equal intervals along the directions of the two coordinate axes of the Frenet coordinate system to perform grid sampling on the road, so as to obtain a series of grid points as the endpoints of the planned trajectory curves.

[0072] For example, if the s axis is taken as the first coordinate axis, Np p mileage points are first sampled at equal intervals along the s axis, that is, the center line direction of the road, and then Np*Nq p *N j grid points are obtained by sampling each of the mileage points at equal intervals along the l axis to the lateral range of the normal direction of the center line of the road.

[0073] Of course, it is also possible to first sample along the l axis at equal intervals and then sample along the s axis at equal intervals, which can also obtain a plurality of endpoints of the planned trajectory curves.

[0074] Further, in one embodiment of the present disclosure, the step of obtaining a plurality of planning trajectory curves according to the start point and a plurality of end points in step S102 comprises:

[0075] Step four, determining a grid start point state vector of the start point and a grid end point state vector of each of the end points in the first coordinate system;

[0076] Step five, fitting the start point and each of the end points respectively by using a polynomial based on the grid start point state vector and each of the grid end point state vectors to obtain a plurality of planning trajectory curves.

[0077] Figure 3 A schematic diagram of a planning trajectory curve in a Frenet coordinate system in an exemplary embodiment of the present disclosure is shown schematically. Referring to Figure 3 In the Frenet coordinate system, a planning trajectory curve can be obtained by connecting the start point and each of the end points, and the space-time grid state of each grid point can be represented by a grid state vector, so the expression of the planning trajectory curve can be obtained by determining the grid state vectors of the start point and the end points.

[0078] It should be noted that in the Frenet coordinate system, the grid state vector can be determined by the lateral distance s pl , the lateral displacement l pl , the heading angle ψ pl , the curvature ρ, the forward speed v along the trajectory tangent direction, and the time t when the vehicle reaches the point, so the state vector of each grid point can be represented by x g = [s pl , l pl , ψ pl , ρ, v, t] T .

[0079] Since the present application focuses on human-machine steering interaction, the speed planning related to the dynamic interaction between the ego vehicle and the surrounding vehicles is appropriately simplified. In each trajectory planning time domain, the speed v is maintained as the current speed of the vehicle, and the time t when the vehicle reaches each grid point is also determined. In addition, the curvature ρ pl can be indirectly represented by the yaw rate , so the state vector of the trajectory grid point becomes

[0080] For the grid start point state vector of the planning trajectory curve at the start point, it can be represented as And the grid end point state vector at the end point can be represented as

[0081] In step four, the state vectors of the start point and the end point of the planning trajectory curve need to be determined respectively.

[0082] Figure 4 Fig. 1 schematically shows a flowchart of a method for determining a grid start state vector according to an example embodiment of the present disclosure. Figure 4 As shown in Fig. 1, the method for determining a grid start state vector includes the following steps:

[0083] Step S401, extracting map information of a location where the vehicle is located;

[0084] Step S402, measuring to obtain measurement data based on the map information;

[0085] Step S403, determining the grid start state vector according to the measurement data.

[0086] Next, steps S401 to S403 will be explained in detail.

[0087] In step S401, first, the map information of the location where the vehicle is located is obtained.

[0088] Specifically, the map information can be a high-precision map, which is a commonly used map information for path planning of unmanned vehicles, also known as an autonomous driving map or a high-resolution map, and is a new map data paradigm for autonomous vehicles.

[0089] The so-called high-precision map is actually a thematic map for serving the autonomous driving system, as opposed to the ordinary navigation electronic map. The absolute position accuracy of the high-precision map is close to 1 m, and the relative position accuracy is in the centimeter level, which can reach 10-20 cm. It can accurately and comprehensively represent road features and requires higher real-time performance. In addition, the high-precision map records specific details of driving behavior, including typical driving behavior, best acceleration and braking points, road condition complexity, and annotations of signal reception conditions for different road segments, etc.

[0090] In step S402, measurement data is obtained by measuring based on the map information.

[0091] In an embodiment of the present disclosure, the measurement data can be obtained by measuring based on the map information, including the lateral distance s pl0 and the lateral displacement l pl0 of the vehicle start point in the first coordinate (Frenet coordinate system), and can also include the lane center line horizontal coordinate x ref0 , the lane center line vertical coordinate y ref0 , the lane center line heading angle ψ ref0 , the lane center line curvature ρ0, and the heading angle ψ, the vehicle speed v x , etc. of the vehicle start point in the second coordinate (global coordinate system).

[0092] In step S403, the grid start-point state vector is determined based on the measurement data.

[0093] Further, determining the grid starting point state vector based on the measurement data includes: extracting the lateral mileage and lateral displacement of the starting point in the first coordinate system based on the measurement data; determining the heading angle of the vehicle in the first coordinate system based on the heading angle of the starting point in the second coordinate system and the heading angle of the lane centerline in the measurement data; calculating the yaw rate of the starting point in the first coordinate system based on the vehicle speed, yaw rate in the second coordinate system, lane centerline curvature, and lateral displacement in the first coordinate system in the measurement data; and determining the grid starting point state vector based on the lateral mileage, lateral displacement, heading angle, and yaw rate of the starting point in the first coordinate system.

[0094] Specifically, the grid starting state vector is Where the horizontal mileage s in the Frenet coordinate system pl0 and lateral displacement l pl0 It can be directly extracted from the measurement data.

[0095] For the heading angle ψ in the first coordinate system pl0 It can be determined based on the heading angle ψ in the second coordinate system and the heading angle ψ of the lane centerline. ref0 To calculate, please refer to formula (1) as follows:

[0096] ψ pl0 =ψ-ψ ref0 (1)

[0097] For the yaw rate in the first coordinate system Based on vehicle speed v x yaw rate in the second coordinate system and the lane centerline curvature ρ0 and the lateral displacement l in the first coordinate system pl0 To calculate, see formula (2) as follows:

[0098]

[0099] Figure 5 This schematically illustrates a flowchart of a method for determining a mesh endpoint state vector according to an exemplary embodiment of this disclosure. (Reference) Figure 5 As shown, the state vector at the end of the grid is determined. The method includes the following steps:

[0100] Step S501: Determine the lateral mileage and lateral displacement of the endpoint in the first coordinate system based on the spatiotemporal grid sampling results;

[0101] Step S502, configure the heading angle and yaw rate of the endpoint in the first coordinate system;

[0102] Step S503, determine the grid endpoint state vector based on the lateral distance, lateral displacement, heading angle and yaw rate of the endpoint in the first coordinate system.

[0103] Specifically, in step S501, since equal-interval sampling is performed along the s axis and the l axis of the Frenet coordinate system when sampling the space-time grid, the lateral distance s pl0 and the lateral displacement l pl0 of each endpoint can be obtained according to the lateral distance s plT and the lateral displacement l plT of the starting point.

[0104] In step S502, the constraint conditions that the heading angle and the yaw rate of the endpoint should satisfy, i.e., ψ plT = 0 and

[0105] Finally, in step S503, the of each endpoint is obtained, forming N p *N j grid states, i.e., the grid state tensor at time k is constructed:

[0106]

[0107] In step five, based on the grid starting point state vector and each grid endpoint state vector, a polynomial is used to fit the starting point and each endpoint to obtain a plurality of planning trajectory curves.

[0108] Specifically, in the Frenet coordinate system, the starting point and the endpoint are connected, and according to the grid starting point state vector and the grid endpoint state vector, the optimal space-time grid point sequence is searched out from the space-time grid, and the point sequence is connected to form a planning trajectory curve.

[0109] The planning trajectory curve can be obtained by polynomial fitting, for example, a sixth-degree polynomial is used to fit and connect the space-time grid points, as shown in formula (3) as follows:

[0110]

[0111] In the formula, b k includes b0=0, b1=ψ pl0 , where Δs=s plT -s pl0 , Δl=l plT -l pl0 , s plT ∈[s pl0 ,s plT ]。

[0112] Figure 6 An exemplary embodiment of the present disclosure is schematically shown in a six polynomial fitting planning trajectory curve diagram. Referring to FIG. 6, the vehicle is currently at the starting point p0, and the time-space grid sampling obtains 8 endpoints p1-p8, respectively connecting p0 and each endpoint and searching for the optimal time-space grid point sequence, which can be fitted to obtain 8 planning trajectory curves as shown in FIG. 6. Figure 6 Figure 6

[0113] In the trajectory planning process, first, the formula (3) is used to obtain the planning trajectory curve based on the Frenet coordinate system, and then the formula (4) and the formula (5) are used to obtain the planning trajectory curve in the global coordinate system, and the formula (4) and (5) are as follows:

[0114] x pl =x ref -l pl sin(ψ ref ) (4)

[0115] y pl =y ref -l pl cos(ψ ref ) (5)

[0116] In the formula, x ref , y ref , ψ ref are the horizontal and vertical coordinates of the lane center line and the tangent direction angle in the global coordinate system.

[0117] In step S103, the trajectory evaluation result is obtained by evaluating the plurality of planning trajectory curves according to a planning trajectory cost function; wherein the planning trajectory cost function includes a first cost function constructed based on the forward prediction result of the driver target trajectory in the co-pilot system of the vehicle and a second cost function constructed based on the backward estimation result of the driver target trajectory.

[0118] In an embodiment of the present disclosure, the plurality of planning trajectory curves obtained above need to be evaluated respectively, and then an optimal target trajectory with the smallest cost function value is searched from the grid state tensor.

[0119] ​​Since the steering operation of the driver and the state of the whole vehicle are affected by the automatic driving system in the co-driving system, the confusion of human-machine decision in the double-agent redundant system will lead to the decrease of the prediction accuracy of the traditional driver intention prediction method. Therefore, under the design principle of the human-centered automatic driving system in the present application, the forward prediction and backward estimation results of the driver target trajectory need to be introduced into the trajectory evaluation function to evaluate the consistency between the trajectory planned by the automatic driving system and the driver target trajectory, so as to reduce the human-machine conflict at the decision layer of the co-driving system.

[0120] Figure 7 A flowchart schematically showing a method for constructing the planning trajectory cost function in an exemplary embodiment of the present disclosure is shown in FIG. 7. Figure 7 As shown in FIG. 7, the steps for constructing the planning trajectory cost function method include:

[0121] Step S701, constructing a first cost function based on the error between the planning trajectory curve and the forward prediction result of the driver target trajectory in the co-driving system;

[0122] Step S702, constructing a second cost function based on the error between the planning trajectory curve and the backward estimation result of the driver target trajectory in the co-driving system;

[0123] Step S703, configuring a first weight of the first cost function and a second weight of the second cost function, respectively;

[0124] Step S704, constructing the planning trajectory cost function according to the first cost function, the second cost function, the first weight and the second weight.

[0125] In step S701, the first cost function J pl1 is constructed. pl1 J is the forward prediction cost function of the driver target trajectory, which represents the error between the vehicle planning trajectory and the forward prediction result of the driver target trajectory, and is specifically shown in formula (6):

[0126]

[0127] In the formula, y pli is the vehicle planning trajectory at the i th preview point (calculated according to formula 5), is the forward prediction result of the driver target trajectory at the i th preview point.

[0128] The first cost function J pl1 The forward prediction of the driver target trajectory can early estimate the lane-changing decision of the driver to obtain the target trajectory in a longer time domain in the future, so as to ensure that the automatic driving system decision layer can plan a more stable and long-time-domain target trajectory.

[0129] In step S702, a second cost function J pl2 is constructed. pl2 The backward estimation cost function of the driver target trajectory represents the error between the planned trajectory and the backward estimation result of the driver target trajectory, and is specifically shown in equation (7):

[0130]

[0131] In the equation, n u is the total number of preview points, y pli is the planned trajectory of the vehicle at the i-th preview point, is the backward estimation result of the driver target trajectory at the i-th preview point.

[0132] The second cost function J pl2 The backward estimation of the driver target trajectory can be used for the target trajectory correction of the automatic driving system and the resynchronization of the human-machine decision-making under the condition of human-machine decision-making divergence caused by the error of forward prediction.

[0133] In step S703, the first weight w1 of the first cost function J pl1 , i.e., the error weight of the forward prediction of the planned trajectory and the driver decision target trajectory, and the second weight w2 of the second cost function, i.e., the error weight of the backward estimation of the planned trajectory and the driver decision target trajectory, need to be set respectively.

[0134] Further, the configuration of the first weight of the first cost function and the second weight of the second cost function includes: calculating the driver steering power and the automatic driving system steering power in the co-driving system to obtain the human-machine steering power difference; and determining the first weight and the second weight based on the human-machine steering power difference.

[0135] Specifically, the first weight w1 and the second weight w2 are determined by the absolute value |P h -P m | of the human-machine steering power difference, and are specifically shown in equations (8) and (9):

[0136]

[0137]

[0138] In the equation, is the weight coefficient of the first weight w1, is the weight coefficient of the second weight w2, h is the steering angle of the driver, D2 is the adjustment parameter of the tan function, P h is the driver steering power, and P mP the is the target steering power of the autonomous driving system.

[0139] Figure 8 A curve diagram illustrating the relationship between the first weight and the second weight in the exemplary embodiment of the present disclosure is schematically shown. Referring to Figure 8 , the abscissa represents the absolute value of the human-machine steering power difference |P h -P m |, and the ordinate represents the weight value, which is between [0, 1]. The solid line represents the prior weight, i.e., the first weight w1, and the dashed line represents the posterior weight, i.e., the second weight w2.

[0140] In step S704, a planning trajectory cost function J is constructed according to the first cost function, the second cost function, the first weight, and the second weight, as shown in formula (10):

[0141] J = w1J pl1 + w2J pl2 (10)

[0142] In one embodiment of the present disclosure, in addition to introducing the forward prediction and backward estimation results of the driver target trajectory, kinematic and dynamic safety, comfort, and passing efficiency factors commonly considered in trajectory planning can also be added.

[0143] Therefore, further, the method for constructing the planning trajectory cost function further comprises: constructing a third cost function based on the lane keeping performance of the planning trajectory curve; constructing a fourth cost function based on the lateral acceleration of the planning trajectory curve; constructing a fifth cost function based on the lateral jerk of the planning trajectory curve; respectively configuring a third weight of the third cost function, a fourth weight of the fourth cost function, and a fifth weight of the fifth cost function; and constructing the planning trajectory cost function according to the first cost function, the second cost function, the third cost function, the fourth cost function, the fifth cost function, the first weight, the second weight, the third weight, the fourth weight, and the fifth weight.

[0144] Specifically, the third cost function J pl3 is a lane keeping performance cost function, representing the kinematic safety of the trajectory; the fourth cost function J pl4 is a lateral acceleration cost function, representing the dynamic safety of the trajectory; and the fifth cost function J pl5 is a lateral jerk cost function, representing the driving smoothness representing the comfort level of the trajectory. J pl3 , J pl4 , and J pl5 The functional expressions of J

[0145]

[0146]

[0147]

[0148] In the formula, N pl represents the total number of trajectory endpoints of a planning process, y refi represents the offset of the reference trajectory on the y-axis, r id represents the steering angle of the vehicle.

[0149] In addition, J pl3 corresponds to a third weight w3, i.e., a lane keeping weight; J pl4 corresponds to a fourth weight w4, i.e., a lateral acceleration penalty weight; and J pl5 corresponds to a fifth weight w5, i.e., a lateral jerk weight.

[0150] Finally, a planning trajectory cost function J is constructed by integrating the first, second, third, fourth, and fifth cost functions as shown in formula (14):

[0151] J = w1J pl1 + w2J pl2 + w3J pl3 + w4J pl4 + w5J pl5 (14)

[0152] After the planning trajectory cost function is constructed, the trajectory cost of each of the plurality of planning trajectory curves obtained in step S2 can be calculated to obtain an evaluation result of each planning trajectory curve.

[0153] In step S104, a target planning trajectory curve is determined from the plurality of planning trajectory curves based on the evaluation result.

[0154] Specifically, the evaluation result of the planning trajectory curve includes the cost of the plurality of planning trajectory curves, and the planning trajectory curve corresponding to the minimum cost is selected as the target planning trajectory curve, so that the vehicle travels with reference to the target planning trajectory curve.

[0155] Based on the above method, when evaluating the trajectory, the planning trajectory cost function is constructed based on the forward prediction result and the backward estimation result of the driver target trajectory for trajectory evaluation, so that the error in the forward prediction of the driver target trajectory can be effectively compensated by the backward estimation result of the driver target trajectory during trajectory evaluation, achieving the purpose of human-machine decision fusion, and realizing human-machine target trajectory rapid synchronization under decision conflict conditions.

[0156] Figure 9A flowchart schematically showing a method for determining a backward estimation result of a driver target trajectory in an exemplary embodiment of the present disclosure is shown. Referring to Figure 9 As shown, determining a backward estimation result of a driver target trajectory in a co-driving system specifically includes the following steps:

[0157] Step S901, constructing a driver trajectory tracking control objective function with the error between a planned trajectory curve and a driver target trajectory being minimized, and constructing a system trajectory tracking control objective function with the error between the planned trajectory curve and an autonomous driving system target trajectory being minimized;

[0158] Step S902, solving the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function based on a Nash equilibrium strategy of distributed model predictive control to obtain the backward estimation result of the driver target trajectory.

[0159] In the present application, since a humanized autonomous driving system steering control strategy is adopted, the entire co-driving system presents a mirror image type architecture, and the mirror image architecture system is suitable for adopting a Nash equilibrium type symmetric game theory to achieve backward estimation of a driver target trajectory.

[0160] Since in feedback Nash equilibrium based on a linear quadratic regulator, the target trajectories of the driver and the decision layer of the autonomous driving system are both located in an augmented system, and through system analysis it is known that the system is not completely observable, which brings difficulties to the design of a state observer. In the Nash equilibrium algorithm based on distributed model predictive control, the human-machine target trajectories are both independently present in their respective objective functions, and are not coupled with other variables in the system, so the driver target trajectory prediction can be achieved based on the Nash equilibrium strategy of distributed model predictive control.

[0161] Next, steps S901 and S902 for determining a backward estimation result of a driver target trajectory in a co-driving system will be described in detail.

[0162] In step S901, a driver trajectory tracking control objective function is constructed with the error between a planned trajectory curve and a driver target trajectory being minimized, and a system trajectory tracking control objective function is constructed with the error between the planned trajectory curve and an autonomous driving system target trajectory being minimized.

[0163] Specifically, constructing a driver trajectory tracking control objective function and a system trajectory tracking control objective function includes the following steps:

[0164] Step one, calculating a predicted lateral displacement of the vehicle;

[0165] Step two, determining the driver preview vehicle lateral displacement at the preset preview distance within the driver control time domain, and constructing the driver trajectory tracking control objective function based on the predicted lateral displacement and the driver preview vehicle lateral displacement; and

[0166] Step three, determining the system preview vehicle lateral displacement at the preset preview distance within the automatic driving system control time domain, and constructing the system trajectory tracking control objective function based on the predicted lateral displacement and the system preview vehicle lateral displacement.

[0167] For step one, in an embodiment of the present disclosure, for the actual system, since the sensor can only obtain the human-machine steering torque at the current time of the vehicle, it is difficult to directly predict the steering behavior of the driver and the automatic driving system within the control time domain in the case that the driver target trajectory is unknown.

[0168] Therefore, the steering control torques of the driver and the automatic driving system within the control time domain can be regarded as constants based on the optimal preview theory, that is, the steering torque of the driver within the control time domain (also referred to as the control strategy of the driver) and the steering torque of the automatic driving system (also referred to as the control strategy of the automatic driving system)

[0169] Therefore, at time k, since the steering torques of the driver and the automatic driving system within the control time domain are constants and the prediction model of the vehicle trajectory is as shown in formula (15):

[0170]

[0171] In the formula, x k is the state vector of the point at time k, that is, the x g of the point at time k; w k ' is the weight of the state vector at time k. In the formula, C c is the system output matrix, and C c = [0, 0, 0, 0, 1, 0] T .

[0172] Thus, the predicted lateral displacement of the vehicle is obtained

[0173] For steps two and three, for the driver and the automatic driving system in the human-machine co-driving system, the respective trajectory tracking control objective functions and are respectively shown in formula (16) and formula (17):

[0174]

[0175]

[0176] wherein, is the prediction model of the vehicle trajectory at time k, and is related to the steering torque of the driver and the steering torque of the autonomous driving system within the control horizon; and are the nth u preview points of the driver and the autonomous driving system control horizon, i.e., the lateral displacement of the vehicle at the farthest preview distance; q Y1 and q Y2 are the weighting coefficients of the trajectory tracking error of the driver and the autonomous driving system; q u1 and q u2 are the penalty coefficients of the steering torque of the driver and the autonomous driving system.

[0177] In step S902, the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function are solved based on the Nash equilibrium strategy of the distributed model predictive control to obtain the backward estimation result of the driver target trajectory.

[0178] Specifically, solving the backward estimation result of the driver target trajectory can be divided into the following steps:

[0179] Step one, obtaining the driver control strategy expression and the autonomous driving system control strategy expression based on the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function;

[0180] Step two, using a convex iteration algorithm to solve the driver control strategy expression and the autonomous driving system control strategy expression to obtain a decoupled human-machine open-loop Nash equilibrium solution;

[0181] Step three, obtaining the lateral displacement expression at the driver preview point according to the human-machine open-loop Nash equilibrium solution;

[0182] Step four, obtaining the least squares solution of the lateral displacement expression by a pseudo-inverse method to obtain the backward estimation result of the driver target trajectory.

[0183] In step one, substituting formula (15) into formula (16) and formula (17) can obtain the control strategy expressions of the driver and the autonomous driving system as shown in formula (18) and formula (19) respectively:

[0184]

[0185]

[0186] In the formula,

[0187] Equations (18) and (19) show that the steering torque control of the driver and the autonomous driving system are coupled. Under the condition of open-loop Nash equilibrium, neither the driver nor the autonomous driving system can further reduce their respective trajectory tracking cost function values ​​by unilaterally changing their own steering control torque, that is, satisfying equations (20) and (21):

[0188]

[0189]

[0190] In the formula, and Let represent the open-loop Nash equilibrium solutions of the control strategies for the driver and the autonomous driving system, respectively.

[0191] In step two, the decoupled human-machine open-loop Nash equilibrium strategy can be obtained based on the convex iterative algorithm, that is, the human-machine open-loop Nash equilibrium solution, as shown in formula (22):

[0192]

[0193] In the formula,

[0194] In step three, expanding formula (22) yields the driver's nth... u The expression for the lateral displacement of the pre-aiming point is shown in formula (23):

[0195]

[0196] As can be seen from formula (23), the lateral displacement at the driver's aiming point can be determined by the interaction between the driver and the steering torque of the autonomous driving system and the target trajectory of the autonomous driving system.

[0197] In step four, the expression for the lateral displacement is solved. Since equation (23) is an overdetermined system of equations, it can be solved using the pseudo-inverse method. The least squares solution, with This is illustrated in formula (24):

[0198]

[0199] In the formula,

[0200] Since formula (24) is based on the steering torque of the driver and the automatic driving system at time k. and and the lateral displacement of the nth preview point of the autonomous driving system u to estimate the driver target trajectory, so as to be called the backward estimation result of the driver target trajectory, the method realizes the backward estimation of the driver target trajectory.

[0201] The backward estimation result of the driver target trajectory sequence of the driver k to k+n u time domain can be calculated according to formula (25):

[0202]

[0203] In the formula, is the backward estimation sequence of the driver target trajectory, R mk is the target trajectory sequence of the autonomous driving system, is the steering torque interaction sequence of the driver and the autonomous driving system k-n u to k moment, X ck is the system state vector sequence, W k is the sequence of disturbances k-n u to k moment, that is:

[0204]

[0205]

[0206]

[0207] In one embodiment of the present disclosure, the forward prediction result of the driver target trajectory can be predicted based on the driver eye movement information, which can be realized by using the prior art, and the present disclosure will not be described in detail here.

[0208] In summary, based on the non-cooperative dynamic game theory, the human-machine steering torque interaction result obtained by the sensor and the real-time target trajectory of the autonomous driving system are comprehensively used to estimate the target trajectory of the driver, and the main innovation points are:

[0209] On the one hand, the backward estimation algorithm of the driver target trajectory based on the non-cooperative game theory is proposed. According to the results of the torque conflict in the human-machine co-driving system, the real decision and target trajectory of the driver are traced back, so it is also called the backward estimation of the driver trajectory. The backward estimation of the driver target trajectory can effectively solve the human-machine decision confusion problem existing in the double-agent redundant system of the driver and the autonomous driving system in the co-driving system, can effectively compensate the error in the forward prediction of the driver target trajectory, and can realize the rapid synchronization of the human-machine target trajectory under the condition of decision conflict.

[0210] ​​On the other hand, a human-machine collaborative trajectory planning method based on a space-time grid method is proposed. The forward prediction result of the target trajectory based on the driver's eye movement information and the backward estimation result of the target trajectory based on the human-machine steering control conflict are innovatively added to the cost function of the automatic driving system trajectory planning, and a time-varying rule of the weight of the forward prediction and backward estimation cost function is designed, which can maximize the deviation between the planned trajectory of the automatic driving system and the target trajectory of the driver, and realize the human-machine decision fusion method considering the driver's eye movement behavior and human-machine steering conflict.

[0211] Therefore, the vehicle trajectory planning method provided by the present disclosure not only improves the decision freedom of the driver, but also reduces the decision disagreement between the driver and the automatic driving system in the co-driving system.

[0212] Figure 10 The composition of a vehicle trajectory planning device in an exemplary embodiment of the present disclosure is schematically shown as follows: Figure 10 As shown in the figure, the vehicle trajectory planning device 1000 can include an acquisition module 1001, a sampling module 1002, an evaluation module 1003, and a determination module 1004. Among them:

[0213] The acquisition module 1001 is configured to take the current position of the vehicle as the starting point of the planned trajectory curve;

[0214] The sampling module 1002 is configured to perform space-time grid sampling based on the starting point to obtain a plurality of ending points of the planned trajectory curve, so as to obtain a plurality of planned trajectory curves according to the starting point and the plurality of ending points;

[0215] The evaluation module 1003 is configured to evaluate the plurality of planned trajectory curves according to a planned trajectory cost function to obtain a trajectory evaluation result; wherein the planned trajectory cost function includes a first cost function constructed based on a forward prediction result of a driver target trajectory in a co-driving system of the vehicle and a second cost function constructed based on a backward estimation result of the driver target trajectory;

[0216] The determination module 1004 is configured to determine a target planned trajectory curve from the plurality of planned trajectory curves based on the evaluation result.

[0217] According to the exemplary embodiment of the present disclosure, the sampling module 1002 is configured to construct a first coordinate system based on the shape of the road where the vehicle is located; sample the starting point along a first coordinate axis of the first coordinate system equidistantly p times to obtain Np mileage points; wherein p is a positive integer; sample each of the mileage points along a second coordinate axis of the first coordinate system equidistantly q times to obtain Np*Nq ending points of the planned trajectory curve; wherein q is a positive integer.

[0218] According to an example embodiment of the present disclosure, the sampling module 1002 is further configured to determine a grid start state vector of the start point and grid end state vectors of the end points in the first coordinate system; and fit the start point and the end points respectively based on the grid start state vector and the grid end state vectors to obtain a plurality of planning trajectory curves using a polynomial.

[0219] According to an example embodiment of the present disclosure, the sampling module 1002 further comprises a start point unit configured to extract map information of a location where the vehicle is located; measure to obtain measurement data based on the map information; and determine the grid start state vector according to the measurement data.

[0220] According to an example embodiment of the present disclosure, the start point unit is configured to extract lateral mileage and lateral displacement of the start point in the first coordinate system based on the measurement data; determine a heading angle of the vehicle in the first coordinate system according to a heading angle of the vehicle in the second coordinate system and a heading angle of the lane center line in the measurement data; calculate a yaw rate of the start point in the first coordinate system according to a vehicle speed of the vehicle, a yaw rate in the second coordinate system, a curvature of the lane center line, and a lateral displacement in the first coordinate system in the measurement data; and determine the grid start state vector based on the lateral mileage, the lateral displacement, the heading angle, and the yaw rate of the start point in the first coordinate system.

[0221] According to an example embodiment of the present disclosure, the sampling module 1002 further comprises an end point unit configured to determine lateral mileage and lateral displacement of the end point in the first coordinate system according to the spatio-temporal grid sampling result; configure a heading angle and a yaw rate of the end point in the first coordinate system; and determine the grid end state vector based on the lateral mileage, the lateral displacement, the heading angle, and the yaw rate of the end point in the first coordinate system.

[0222] According to an example embodiment of the present disclosure, the evaluation module 1003 further comprises a function unit configured to construct a first cost function based on an error between the planning trajectory curve and a forward prediction result of a driver target trajectory in the co-driving system; construct a second cost function based on an error between the planning trajectory curve and a backward estimation result of the driver target trajectory in the co-driving system; configure a first weight of the first cost function and a second weight of the second cost function respectively; and construct the planning trajectory cost function according to the first cost function, the second cost function, the first weight, and the second weight.

[0223] According to an example embodiment of the present disclosure, the function unit is further configured to calculate a driver steering power and an automatic driving system steering power in the co-driving system to obtain a human-machine steering power difference; and determine the first weight and the second weight based on the human-machine steering power difference.

[0224] According to an example embodiment of the present disclosure, the function unit is further configured to construct a third cost function based on lane keeping of the planned trajectory curve, construct a fourth cost function based on lateral acceleration of the planned trajectory curve, construct a fifth cost function based on lateral jerk of the planned trajectory curve, configure a third weight of the third cost function, a fourth weight of the fourth cost function, and a fifth weight of the fifth cost function respectively, and construct the planned trajectory cost function according to the first cost function, the second cost function, the third cost function, the fourth cost function, the fifth cost function, the first weight, the second weight, the third weight, the fourth weight, and the fifth weight.

[0225] According to an example embodiment of the present disclosure, the evaluation module 1003 further comprises a backward estimation unit configured to construct a driver trajectory tracking control target function with minimum error between the planned trajectory curve and the driver target trajectory, and construct a system trajectory tracking control target function with minimum error between the planned trajectory curve and the automatic driving system target trajectory, and solve the driver trajectory tracking control target function and the automatic driving system trajectory tracking control target function based on a Nash equilibrium strategy of distributed model predictive control to obtain a backward estimation result of the driver target trajectory.

[0226] According to an example embodiment of the present disclosure, the backward estimation unit is further configured to calculate a predicted lateral displacement of the vehicle, determine a driver preview vehicle lateral displacement at a preset preview distance within a driver control time domain, and construct the driver trajectory tracking control target function based on the predicted lateral displacement and the driver preview vehicle lateral displacement, and determine a system preview vehicle lateral displacement at the preset preview distance within an automatic driving system control time domain, and construct the system trajectory tracking control target function based on the predicted lateral displacement and the system preview vehicle lateral displacement.

[0227] According to an example embodiment of the present disclosure, the backward estimation unit is further configured to obtain a driver control strategy expression and an automatic driving system control strategy expression based on the driver trajectory tracking control target function and the automatic driving system trajectory tracking control target function, solve the driver control strategy expression and the automatic driving system control strategy expression by using a convex iteration algorithm to obtain a decoupled human-machine open-loop Nash equilibrium solution, obtain a lateral displacement expression at a driver preview point according to the human-machine open-loop Nash equilibrium solution, and obtain the backward estimation result of the driver target trajectory by solving a least square solution of the lateral displacement expression by using a pseudo-inverse method.

[0228] The specific details of each module in the vehicle trajectory planning device 1000 described above have been described in detail in the corresponding vehicle trajectory planning method, and thus will not be described here again.

[0229] It should be noted that although several modules or units of the device for action execution are mentioned in the foregoing detailed description, such division is not mandatory. Indeed, according to embodiments of the disclosure, features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, features and functions of one module or unit described above can be further divided into embodied by multiple modules or units.

[0230] In an exemplary embodiment of the disclosure, a storage medium capable of implementing the above method is also provided. Figure 11 The schematic diagram of a computer readable storage medium in an exemplary embodiment of the disclosure is schematically shown as Figure 11 As shown, a program product 1100 for implementing the above method according to embodiments of the disclosure is described, which can adopt a portable compact disc read-only memory (CD-ROM) and include program codes, and can run on a terminal device, such as a mobile phone. However, the program product of the disclosure is not limited thereto, and in this document, the readable storage medium can be any tangible medium containing or storing programs, which can be used or combined with an instruction execution system, device or apparatus.

[0231] In an exemplary embodiment of the disclosure, an electronic device capable of implementing the above method is also provided. Figure 12 The structural schematic diagram of a computer system of an electronic device in an exemplary embodiment of the disclosure is schematically shown.

[0232] It should be noted that, Figure 12 The computer system 1200 of the electronic device shown is only an example, and should not bring any limitation to the functions and use range of embodiments of the disclosure.

[0233] As Figure 12 shown, the computer system 1200 includes a central processing unit (CPU) 1201, which can perform various appropriate actions and processes according to programs stored in a read-only memory (ROM) 1202 or programs loaded from a storage portion 1208 into a random access memory (RAM) 1203. In the RAM 1203, various programs and data required for system operation are also stored. The CPU 1201, the ROM 1202 and the RAM 1203 are connected to each other through a bus 1204. An input / output (I / O) interface 1205 is also connected to the bus 1204.

[0234] The following components are connected to the I / O interface 1205: an input part 1206 including a keyboard, a mouse, etc.; an output part 1207 including a display such as a Cathode Ray Tube (CRT), a Liquid Crystal Display (LCD), etc., and a speaker, etc.; a storage part 1208 including a hard disk, etc.; and a communication part 1209 including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication part 1209 performs communication processing via a network such as the Internet. A drive 1210 is also connected to the I / O interface 1205 as necessary. A removable medium 1211 such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc. is attached to the drive 1210 as necessary, so that a computer program read out therefrom is installed in the storage part 1208 as necessary.

[0235] In particular, according to embodiments of the present disclosure, the processes described below with reference to flowcharts can be implemented as a computer software program. For example, embodiments of the present disclosure include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for executing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network by the communication part 1209, and / or installed from the removable medium 1211. When the computer program is executed by the central processing unit (CPU) 1201, various functions defined in the system of the present disclosure are executed.

[0236] It should be noted that the computer-readable medium in the embodiments of the present disclosure can be a computer-readable signal medium or a computer-readable storage medium or any combination of the two. The computer-readable storage medium may, for example, but is not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or apparatus, or any combination thereof. More specific examples of the computer-readable storage medium can include, but are not limited to, an electrical connection having one or more wires, a portable computer diskette, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), a flash memory, an optical fiber, a portable compact disk read-only memory (Compact Disc Read-Only Memory, CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, the computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, device or apparatus. In the present disclosure, the computer-readable signal medium can include a data signal carried in a baseband or as a part of a carrier wave, which carries computer-readable program code. Such a propagated data signal can take various forms, including but not limited to an electromagnetic signal, an optical signal, or any suitable combination thereof. The computer-readable signal medium can also be any computer-readable medium other than the computer-readable storage medium, which can send, propagate or transmit a program for use by or in conjunction with an instruction execution system, device or apparatus. The program code contained on the computer-readable medium can be transmitted by any suitable medium, including but not limited to wireless, wired, or the like, or any suitable combination thereof.

[0237] The flowcharts and block diagrams in the drawings illustrate the possible implementation architectures, functions and operations of the systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowcharts or block diagrams can represent a module, a program segment, or a portion of code that contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions noted in the blocks can occur in different orders than that shown in the drawings. For example, two blocks that are shown in succession can actually be executed substantially concurrently, or they can be executed in reverse order, depending on the functionality involved. It should also be noted that each block in the block diagrams or flowcharts, and combinations of blocks in the block diagrams or flowcharts, can be implemented by a dedicated hardware-based system that performs specified functions or operations, or can be implemented by a combination of special-purpose hardware and computer instructions.

[0238] The units described in the embodiments of the present disclosure can be implemented in the form of software, or can be implemented in the form of hardware, and the described units can also be arranged in a processor. In some cases, the names of these units do not constitute a limitation on the units themselves.

[0239] As another aspect, the present disclosure also provides a computer readable medium, which can be included in the electronic device described in the above embodiments, or can exist independently without being assembled into the electronic device. The above computer readable medium carries one or more programs, which, when executed by the electronic device, enable the electronic device to implement the method described in the above embodiments.

[0240] It should be noted that although several modules or units of the device for action execution are mentioned in the above detailed description, such division is not mandatory. In fact, according to the embodiments of the present disclosure, the features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided into a plurality of modules or units.

[0241] From the above description of the embodiments, those skilled in the art can easily understand that the example embodiments described herein can be implemented by software, or by software in combination with necessary hardware. Therefore, the technical solutions according to the embodiments of the present disclosure can be embodied in the form of a software product. The software product can be stored in a non-volatile storage medium (which can be a CD-ROM, a U disk, a mobile hard disk, etc.) or a network, and includes a number of instructions to make a computing device (which can be a personal computer, a server, a touch terminal, or a network device, etc.) execute the methods according to the embodiments of the present disclosure.

[0242] Other embodiments of the present disclosure will be apparent to those skilled in the art from consideration of the specification and practice of the application disclosed herein. The present disclosure is intended to cover any variations, uses, or adaptations of the present disclosure following the general principles thereof and including modifications and equivalents of the present disclosure that are obvious to those skilled in the art.

[0243] It should be understood that the present disclosure is not limited to the precise structures described and shown in the above description and the accompanying drawings, and various modifications and changes can be made without departing from the scope thereof. The scope of the present disclosure is limited only by the appended claims.

Claims

1. A vehicle trajectory planning method, characterized in that, include: Use the vehicle's current position as the starting point of the planned trajectory curve; Based on the starting point, spatiotemporal grid sampling is performed to obtain the endpoints of multiple planned trajectory curves, so as to obtain multiple planned trajectory curves according to the starting point and multiple endpoints; The multiple planned trajectory curves are evaluated according to the planned trajectory cost function to obtain the trajectory evaluation result; wherein, the planned trajectory cost function includes a first cost function constructed based on the forward prediction result of the driver's target trajectory in the vehicle's co-driving system and a second cost function constructed based on the backward estimation result of the driver's target trajectory; Based on the evaluation results, a target planning trajectory curve is determined from the multiple planning trajectory curves; The first cost function is: The second cost function is: In the formula, n u y represents the total number of aiming points. pli Plan the trajectory for the vehicle at the i-th aiming point. This represents the forward prediction result of the driver's target trajectory at the i-th aiming point. This is the backward estimation result of the driver's target trajectory at the i-th aiming point.

2. The vehicle trajectory planning method according to claim 1, characterized in that, The endpoints of the multiple planned trajectory curves obtained by spatiotemporal grid sampling based on the starting point include: A first coordinate system is constructed based on the shape of the road where the vehicle is located; The starting point is sampled p times at equal intervals along the first coordinate axis of the first coordinate system to obtain Np mileage points; where p is a positive integer. Each of the aforementioned mileage points is sampled q times at equal intervals along the second coordinate axis of the first coordinate system to obtain the endpoints of Np*Nq planned trajectory curves; where q is a positive integer.

3. The vehicle trajectory planning method according to claim 2, characterized in that, The process of obtaining multiple planned trajectory curves based on the starting point and multiple ending points includes: Determine the grid start-point state vector of the starting point and the grid end-point state vector of each of the ending points in the first coordinate system; Based on the grid starting point state vector and the grid ending point state vector, a polynomial is used to fit the starting point and the ending point respectively to obtain multiple planned trajectory curves.

4. The vehicle trajectory planning method according to claim 3, characterized in that, The method further includes: determining the grid start-point state vector, wherein determining the grid start-point state vector includes: Extract map information showing the location of the vehicle; Measurement data is obtained by measuring based on the map information; The grid start-point state vector is determined based on the measurement data.

5. The vehicle trajectory planning method according to claim 4, characterized in that, Determining the grid start-point state vector based on the measurement data includes: Based on the measurement data, the lateral mileage and lateral displacement of the starting point in the first coordinate system are extracted; The heading angle of the vehicle in the first coordinate system is determined based on the heading angle of the starting point in the second coordinate system and the heading angle of the lane centerline in the measurement data. The yaw rate of the starting point in the first coordinate system is calculated based on the vehicle speed, yaw rate in the second coordinate system, lane centerline curvature, and lateral displacement in the first coordinate system from the measured data. The grid starting point state vector is determined based on the lateral mileage, lateral displacement, heading angle, and yaw rate of the starting point in the first coordinate system.

6. The vehicle trajectory planning method according to claim 3, characterized in that, The method further includes: determining the grid endpoint state vector, wherein determining the grid endpoint state vector includes: The lateral mileage and lateral displacement of the endpoint in the first coordinate system are determined based on the spatiotemporal grid sampling results. Configure the heading angle and yaw rate of the endpoint in the first coordinate system; The grid endpoint state vector is determined based on the lateral mileage, lateral displacement, heading angle, and yaw rate of the endpoint in the first coordinate system.

7. The vehicle trajectory planning method according to claim 1, characterized in that, The method further includes: constructing the planned trajectory cost function, wherein constructing the planned trajectory cost function includes: A first cost function is constructed based on the error between the planned trajectory curve and the forward prediction result of the driver's target trajectory in the co-driving system; A second cost function is constructed based on the error between the planned trajectory curve and the backward estimation result of the driver's target trajectory in the co-driving system; Configure the first weight of the first cost function and the second weight of the second cost function respectively; The planned trajectory cost function is constructed based on the first cost function, the second cost function, the first weight, and the second weight.

8. The vehicle trajectory planning method according to claim 7, characterized in that, The step of configuring the first weight of the first cost function and the second weight of the second cost function respectively includes: Calculate the driver's steering power and the autonomous driving system's steering power in the co-driving system to obtain the human-machine steering power difference; The first weight and the second weight are determined based on the difference in human-machine steering power.

9. The vehicle trajectory planning method according to claim 7, characterized in that, The step of constructing the planned trajectory cost function based on the first cost function, the second cost function, the first weight, and the second weight includes: Construct a third cost function based on lane keeping performance of the planned trajectory curve; A fourth cost function is constructed based on the lateral acceleration of the planned trajectory curve; The fifth cost function is constructed based on the lateral jerk of the planned trajectory curve; Configure the third weight of the third cost function, the fourth weight of the fourth cost function, and the fifth weight of the fifth cost function respectively; The planned trajectory cost function is constructed based on the first cost function, the second cost function, the third cost function, the fourth cost function, the fifth cost function, the first weight, the second weight, the third weight, the fourth weight, and the fifth weight.

10. The vehicle trajectory planning method according to claim 7, characterized in that, The method further includes: determining the backward estimation result of the driver's target trajectory in the shared driving system, wherein determining the backward estimation result of the driver's target trajectory in the shared driving system includes: The objective function for driver trajectory tracking control is constructed by minimizing the error between the planned trajectory curve and the driver's target trajectory, and the objective function for system trajectory tracking control is constructed by minimizing the error between the planned trajectory curve and the target trajectory of the autonomous driving system. The backward estimation result of the driver's target trajectory is obtained by solving the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function based on the Nash equilibrium strategy of distributed model predictive control.

11. The vehicle trajectory planning method according to claim 10, characterized in that, The objective function for driver trajectory tracking control, which is constructed by minimizing the error between the planned trajectory curve and the driver's target trajectory, and the objective function for system trajectory tracking control, which is constructed by minimizing the error between the planned trajectory curve and the autonomous driving system, include: Calculate the predicted lateral displacement of the vehicle; Determine the driver's predicted lateral displacement of the vehicle at a preset aiming distance within the driver control time domain, and construct the driver trajectory tracking control objective function based on the predicted lateral displacement and the driver's predicted lateral displacement of the vehicle; and The system predicts the lateral displacement of the vehicle at the preset predictive distance within the control time domain of the autonomous driving system, and constructs the system trajectory tracking control objective function based on the predicted lateral displacement and the system predictive lateral displacement.

12. The vehicle trajectory planning method according to claim 10, characterized in that, The Nash equilibrium strategy based on distributed model predictive control solves the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function to obtain the backward estimation result of the driver's target trajectory, including: Based on the driver trajectory tracking control objective function and the autonomous driving system trajectory tracking control objective function, the driver control strategy expression and the autonomous driving system control strategy expression are obtained; The decoupled human-machine open-loop Nash equilibrium solution is obtained by solving the driver control strategy expression and the autonomous driving system control strategy expression using the convex iterative algorithm. The expression for the lateral displacement at the driver's aiming point is obtained based on the open-loop Nash equilibrium solution of the human-machine interface. The backward estimation result of the driver's target trajectory is obtained by solving the least squares solution of the lateral displacement expression using the pseudo-inverse method.

13. A vehicle trajectory planning device, characterized in that, include: The acquisition module is used to take the vehicle's current position as the starting point of the planned trajectory curve; The sampling module is used to perform spatiotemporal grid sampling based on the starting point to obtain the endpoints of multiple planned trajectory curves, so as to obtain multiple planned trajectory curves based on the starting point and multiple endpoints; An evaluation module is used to evaluate the multiple planned trajectory curves according to the planned trajectory cost function to obtain a trajectory evaluation result; wherein, the planned trajectory cost function includes a first cost function constructed based on the forward prediction result of the driver's target trajectory in the vehicle's co-driving system and a second cost function constructed based on the backward estimation result of the driver's target trajectory; A determination module is used to determine a target planning trajectory curve from the plurality of planning trajectory curves based on the evaluation results; The first cost function is: The second cost function is: In the formula, n u y represents the total number of aiming points. pli Plan the trajectory for the vehicle at the i-th aiming point. This represents the forward prediction result of the driver's target trajectory at the i-th aiming point. This is the backward estimation result of the driver's target trajectory at the i-th aiming point.

14. A computer-readable storage medium having a computer program stored thereon, the program being executed by a processor to implement the vehicle trajectory planning method as described in any one of claims 1 to 12.

15. An electronic device, characterized in that, include: One or more processors; A storage device for storing one or more programs, which, when executed by one or more processors, cause the one or more processors to implement the vehicle trajectory planning method as described in any one of claims 1 to 12.

Images