CN112389427A

CN112389427A - Vehicle track optimization method and device, electronic equipment and storage medium

Info

Publication number: CN112389427A
Application number: CN202110069601.8A
Authority: CN
Inventors: 由长喜
Original assignee: Tencent Technology Shenzhen Co Ltd
Current assignee: Tencent Technology Shenzhen Co Ltd
Priority date: 2021-01-19
Filing date: 2021-01-19
Publication date: 2021-02-23
Anticipated expiration: 2041-01-19
Also published as: CN112389427B

Abstract

The embodiment of the application discloses a vehicle track optimization method, a vehicle track optimization device, electronic equipment and a storage medium; the method and the device for controlling the vehicle to run in the long-time environment can obtain current position information of the vehicle and an initial reference track planned in a preset time length, determine characteristic constraint information of running characteristics of the vehicle in the initial reference track, calculate basic cost of the initial reference track, identify at least one target obstacle of the vehicle, calculate obstacle cost of the initial reference track according to the initial reference track and a predicted running track of each target obstacle, calculate final value cost of the initial reference track according to a current system state and a target system state of the vehicle, determine global cost of the initial reference track based on the basic cost, the obstacle cost and the final value cost, update the initial reference track by using the global cost to obtain an updated reference track, and control the vehicle to run according to the updated reference track. The scheme can effectively improve the driving safety of the vehicle.

Description

Vehicle track optimization method and device, electronic equipment and storage medium

Technical Field

The application relates to the technical field of unmanned driving, in particular to a vehicle track optimization method and device, electronic equipment and a storage medium.

Background

In recent years, along with the development of artificial intelligence, unmanned driving technology of vehicles has attracted much attention. The vehicle path planning is one of key technologies for realizing unmanned driving of a vehicle, and the driving track planning of the unmanned vehicle is to plan an effective path which is free of collision and can safely reach a target point according to performance indexes after a starting point and the target point of the unmanned vehicle are given on the basis of a certain environment model.

After planning a driving track, unmanned vehicle begins to travel according to the planned driving track, but when meeting the urgent stifled or need promptly dodge of barrier on the way of traveling, prior art often avoids the collision through the high strength brake, and this makes the vehicle keep away the barrier ability relatively poor when meeting dynamic barrier, is unfavorable for the safety of traveling of vehicle.

Disclosure of Invention

The embodiment of the application provides a vehicle track optimization method and device, electronic equipment and a storage medium, and the safety of vehicle running can be effectively improved.

The embodiment of the application provides a vehicle track optimization method, which comprises the following steps:

acquiring current position information of a vehicle and an initial reference track planned within a preset time length;

determining characteristic constraint information of the driving characteristics of the vehicle in the initial reference track, and calculating the basic cost of the initial reference track based on the characteristic constraint information;

identifying at least one target obstacle of the vehicle based on the current position information, and calculating an obstacle cost of the initial reference trajectory according to the initial reference trajectory and a predicted travel trajectory of each target obstacle;

calculating a final value cost of the initial reference track according to the current system state and a target system state of the vehicle, wherein the target system state is a state expected to be reached by the vehicle after a preset time;

determining a global cost of the initial reference trajectory based on the basic cost, the obstacle cost and the final value cost;

and updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

Correspondingly, the embodiment of the present application further provides a vehicle trajectory optimization device, including:

the system comprises an acquisition unit, a processing unit and a control unit, wherein the acquisition unit is used for acquiring current position information of a vehicle and an initial reference track planned in a preset time length;

a first calculation unit, configured to determine feature constraint information of a driving feature of the vehicle in the initial reference trajectory, and calculate a basic cost of the initial reference trajectory based on the feature constraint information;

a second calculation unit, configured to identify at least one target obstacle of the vehicle based on the current position information, and calculate an obstacle cost of the initial reference trajectory according to the initial reference trajectory and a predicted travel trajectory of each target obstacle;

the third calculating unit is used for calculating the final value cost of the initial reference track according to the current system state and the target system state of the vehicle, wherein the target system state is a state which the vehicle is expected to reach after a preset time;

a determining unit, configured to determine a global cost of the initial reference trajectory based on the basic cost, the obstacle cost, and the final value cost;

and the updating unit is used for updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

Optionally, in some embodiments, the second calculating unit may include an identifying subunit, as follows:

the identification subunit is specifically configured to determine, according to the current location information, a current lane in which the vehicle is located; respectively acquiring adjacent vehicles meeting a preset distance in a current lane and adjacent lanes; calculating a time of collision of the vehicle with the neighboring vehicle; and determining the adjacent vehicle with the collision time meeting the preset threshold value as the target obstacle of the vehicle.

Optionally, in some embodiments, the preset time period includes at least one target time, and the second calculating unit may include a calculating subunit, as follows:

the calculation subunit is configured to construct a collision polygon of the vehicle and the target obstacle at the at least one target time based on the initial reference trajectory and the predicted travel trajectory of the target obstacle; calculating a closest distance of the vehicle to the collision polygon at the at least one target time; calculating an instantaneous collision cost of the vehicle with the target obstacle based on the closest distance; and calculating the obstacle cost of the initial reference track according to the instantaneous collision cost.

Optionally, in some embodiments, the calculating subunit may be specifically configured to determine shape information of the vehicle at a target time based on an initial reference trajectory of the vehicle; determining shape information of the target obstacle at a target time based on the predicted travel track of the target obstacle; -calculating a minkowski sum of said vehicle and said target obstacle at a target moment in time using shape information of said vehicle and shape information of said target obstacle, -constructing a collision polygon from said minkowski sum.

Optionally, in some embodiments, the calculating subunit may be specifically configured to determine a target vertex at which the vehicle is closest to the collision polygon at the target time; dividing the plane space of the vehicle into at least one area by taking the target vertex as a center; and determining a target area where the vehicle is located, and calculating the closest distance between the vehicle and the collision polygon based on the distance from the midpoint of the vehicle to the target vertex and the target area.

Optionally, in some embodiments, the driving characteristics include driving energy, the basic cost includes energy cost, and the first calculation unit may be specifically configured to obtain a preset energy weight, and determine energy constraint information of the driving energy of the vehicle in the initial reference trajectory; an energy cost of the initial reference trajectory is calculated based on the energy constraint information.

Optionally, in some embodiments, the driving characteristics include a driving speed, the basic cost includes a speed cost, and the first calculation unit may be specifically configured to obtain a current driving speed of the vehicle and a preset speed weight; determining a target vehicle speed of the vehicle after a preset time; and calculating the vehicle speed cost of the initial reference track based on the current running vehicle speed of the vehicle, the target vehicle speed and a preset vehicle speed weight.

Optionally, in some embodiments, the first calculating unit may be specifically configured to obtain a following distance between the vehicle and a guided vehicle, a lane speed limit of a current lane of the vehicle, and a state of the guided vehicle; and determining the target vehicle speed after the preset time length of the vehicle based on the following distance, the lane speed limit and the guiding vehicle state.

Optionally, in some embodiments, the driving characteristics include lane boundaries, the basic cost includes a boundary cost, and the first calculation unit may be specifically configured to determine a lane left boundary and a lane right boundary of the vehicle according to the current position information of the vehicle; determining boundary constraint information of the vehicle in the initial reference trajectory based on the lane left boundary and the lane right boundary; calculating a boundary cost of the initial reference trajectory based on the boundary constraint information.

Optionally, in some embodiments, the driving characteristics include vehicle acceleration, the basic cost includes an acceleration cost, and the first calculation unit may be specifically configured to determine a maximum acceleration and a minimum acceleration of the vehicle acceleration in the initial reference trajectory; and calculating the acceleration cost of the initial reference track based on the minimum acceleration and the maximum acceleration.

Optionally, in some embodiments, the driving characteristics include a vehicle yaw rate, the basic cost includes a cost of an angular velocity, and the first calculation unit may be specifically configured to determine a minimum yaw rate and a maximum yaw rate of the vehicle in the initial reference trajectory; based on the minimum yaw-rate and the maximum yaw-rate, an angular-rate cost of the initial reference trajectory is calculated.

Optionally, in some embodiments, the preset duration includes at least one target time, and the updating unit may be specifically configured to, if the global cost does not satisfy the convergence condition, reversely calculate, from a last target time of the preset duration, an optimal control rate of each target time in the initial reference trajectory; determining target track points according to the optimal control rate of each target moment; and updating the initial reference track based on the target track point until the global cost meets a convergence condition to obtain an updated reference track.

In addition, the embodiment of the present application further provides a computer-readable storage medium, where a plurality of instructions are stored, and the instructions are adapted to be loaded by a processor to perform the steps in any one of the vehicle trajectory optimization methods provided by the embodiments of the present application.

In addition, an electronic device is further provided in an embodiment of the present application, and includes a memory, a processor, and a computer program stored on the memory and executable on the processor, where the processor executes the program to implement the steps in any one of the vehicle trajectory optimization methods provided in the embodiments of the present application.

In addition, the embodiment of the present application further provides a vehicle navigation device, which includes a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps in any one of the vehicle trajectory optimization methods provided by the embodiments of the present application.

In addition, the embodiment of the application also provides an unmanned vehicle, which comprises a driving system, a control system and a navigation system; the navigation system is used for executing any vehicle track optimization method provided by the embodiment of the application to navigate the vehicle; the control system is used for controlling the driving system under the navigation of the navigation system; the driving system is used for driving the vehicle to move under the control of the control system.

According to one aspect of the application, there is provided a computer program product or computer program comprising computer instructions stored in a computer-readable storage medium, the computer instructions being read by a processor of a computer device from the computer-readable storage medium, the computer instructions being executable by the processor to cause the computer device to perform the method provided in the various alternative implementations of the vehicle trajectory optimization aspect described above.

The embodiment may obtain current position information of a vehicle and an initial reference track planned within a preset time period, then determine feature constraint information of driving features of the vehicle in the initial reference track, calculate a basic cost of the initial reference track based on the feature constraint information, identify at least one target obstacle of the vehicle based on the current position information, calculate an obstacle cost of the initial reference track according to the initial reference track and a predicted driving track of each target obstacle, calculate a final value cost of the initial reference track according to a current system state and a target system state of the vehicle, where the target system state is a state expected to be reached by the vehicle after the preset time period, and then determine a global cost of the initial reference track based on the basic cost, the obstacle cost and the final value cost, and then, updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track. The scheme can effectively improve the driving safety of the vehicle.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.

FIG. 1a is a schematic view of a vehicle trajectory optimization scenario provided by an embodiment of the present application;

FIG. 1b is a first flowchart of a vehicle trajectory optimization method provided by an embodiment of the present application;

FIG. 2a is a schematic view of a local coordinate system of a vehicle according to an embodiment of the present application;

FIG. 2b is a schematic diagram of a target obstacle of the vehicle provided by the embodiments of the present application;

FIG. 2c is a schematic diagram of a constructed collision polygon provided by an embodiment of the present application;

FIG. 2d is a schematic diagram illustrating a distance calculation between a vehicle and a collision polygon according to an embodiment of the present application;

FIG. 2e is a second flowchart of a vehicle trajectory optimization method provided by the embodiment of the present application;

FIG. 2f is a schematic diagram of vehicle trajectory optimization provided by an embodiment of the present application;

FIG. 2g is a third flowchart of a vehicle trajectory optimization method provided by the embodiment of the present application;

FIG. 3 is a schematic structural diagram of a vehicle trajectory optimization device provided by an embodiment of the present application;

fig. 4 is a schematic structural diagram of an electronic device provided in an embodiment of the present application;

FIG. 5 is a schematic structural diagram of a vehicle navigation device provided in an embodiment of the present application;

fig. 6 is a schematic structural diagram of an unmanned vehicle provided in an embodiment of the present application.

Detailed Description

The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

The principles of the present application are illustrated as being implemented in a suitable computing environment. In the description that follows, specific embodiments of the present application will be described with reference to steps and symbols executed by one or more computers, unless otherwise indicated. Accordingly, these steps and operations will be referred to, several times, as being performed by a computer, the computer performing operations involving a processing unit of the computer in electronic signals representing data in a structured form. This operation transforms the data or maintains it at locations in the computer's memory system, which may be reconfigured or otherwise altered in a manner well known to those skilled in the art. The data maintains a data structure that is a physical location of the memory that has particular characteristics defined by the data format. However, while the principles of the application have been described in language specific to above, it is not intended to be limited to the specific form set forth herein, and it will be recognized by those of ordinary skill in the art that various of the steps and operations described below may be implemented in hardware.

The term "unit" as used herein may be considered a software object executing on the computing system. The various components, units, engines, and services described herein may be viewed as objects of implementation on the computing system. The apparatus and method described herein may be implemented in software, or may be implemented in hardware, and are within the scope of the present application.

The terms "first", "second", and "third", etc. in this application are used to distinguish between different objects and not to describe a particular order. Furthermore, the terms "include" and "have," as well as any variations thereof, are intended to cover non-exclusive inclusions. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those steps or elements listed, but rather, some embodiments may include other steps or elements not listed or inherent to such process, method, article, or apparatus.

Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the application. The appearances of the phrase in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. It is explicitly and implicitly understood by one skilled in the art that the embodiments described herein can be combined with other embodiments.

The embodiment of the application provides a vehicle track optimization method and device, electronic equipment and a storage medium. The vehicle trajectory optimization device may be integrated in an Electronic device, and the Electronic device may be a server, or may be a device such as an on-board terminal (Electronic Control Unit, ECU).

For example, as shown in fig. 1a, first, the electronic device integrated with the vehicle trajectory optimization device may obtain current position information of a vehicle and an initial reference trajectory planned within a preset time period, then determine feature constraint information of a driving feature of the vehicle in the initial reference trajectory, calculate a basic cost of the initial reference trajectory based on the feature constraint information, identify at least one target obstacle of the vehicle based on the current position information, calculate an obstacle cost of the initial reference trajectory according to the initial reference trajectory and a predicted driving trajectory of each target obstacle, calculate a final cost of the initial reference trajectory according to a current system state of the vehicle and a target system state, where the vehicle is expected to reach after the preset time period, and then, based on the basic cost, And determining the global cost of the initial reference track according to the obstacle cost and the final value cost, then updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

The embodiment of the application provides a vehicle track optimization method, and relates to an unmanned technology in the field of artificial intelligence. Among them, Artificial Intelligence (AI) is a theory, method, technique and application system that simulates, extends and expands human Intelligence using a digital computer or a machine controlled by a digital computer, senses the environment, acquires knowledge and uses the knowledge to obtain the best result. In other words, artificial intelligence is a comprehensive technique of computer science that attempts to understand the essence of intelligence and produce a new intelligent machine that can react in a manner similar to human intelligence. Artificial intelligence is the research of the design principle and the realization method of various intelligent machines, so that the machines have the functions of perception, reasoning and decision making. The artificial intelligence technology is a comprehensive subject and relates to the field of extensive technology, namely the technology of a hardware level and the technology of a software level. The artificial intelligence software technology mainly comprises a computer vision technology, a machine learning/deep learning direction and the like.

The unmanned technology is a comprehensive body of multiple leading-edge subjects such as a sensor, a computer, artificial intelligence, communication, navigation positioning, mode recognition, machine vision, intelligent control and the like, and refers to a technology which can guide and decide a vehicle driving task without testing the physical driving operation of a driver, replace the testing of the control behavior of the driver and enable the vehicle to complete the function of safe driving. According to the function module of the unmanned automobile, the key technologies of the unmanned automobile comprise environment perception, navigation positioning, path planning, decision control and the like. The scheme mainly relates to path planning, and the path planning is a bridge for information perception and intelligent control of unmanned vehicles and is a basis for realizing autonomous driving. The task of path planning is to find a collision-free path from an initial state including a position and a posture to a target state according to a certain evaluation standard in an environment with obstacles.

According to the scheme, the barrier cost is evaluated according to the initial reference track of the vehicle and the predicted running track of the target barrier, so that the initial reference track is optimized through the barrier cost, the basic cost and the final value cost, the vehicle can obtain the optimal running track at the minimum running cost under an emergency condition, the running safety of the vehicle is effectively improved, and the energy consumption of the vehicle is saved. According to the scheme, the unmanned vehicle planning track is subjected to horizontal and vertical joint optimization in the space-time field, so that the stability, feasibility, comfortableness and safety of vehicle track planning are better met, the lane space is fully utilized to strive for braking time, and meanwhile, the cost control requirement is met, so that the unmanned vehicle can flexibly avoid obstacles in an emergency state, and the running safety of the unmanned vehicle is greatly improved.

The following are detailed below. It should be noted that the following description of the embodiments is not intended to limit the preferred order of the embodiments.

The embodiment will be described in terms of a vehicle trajectory optimization device, which may be specifically integrated in an electronic device, where the electronic device may be a server, or may also be a device such as a vehicle-mounted terminal.

A vehicle trajectory optimization method, comprising: acquiring current position information of a vehicle and an initial reference track planned in a preset time period, then determining characteristic constraint information of running characteristics of the vehicle in the initial reference track, calculating a basic cost of the initial reference track based on the characteristic constraint information, identifying at least one target obstacle of the vehicle based on the current position information, calculating an obstacle cost of the initial reference track according to the initial reference track and a predicted running track of each target obstacle, calculating a final value cost of the initial reference track according to a current system state and a target system state of the vehicle, wherein the target system state is a state expected to be reached by the vehicle in the later period of the preset time period, and then determining a global cost of the initial reference track based on the basic cost, the obstacle cost and the final value cost, and then, updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

As shown in fig. 1b, the vehicle trajectory optimization method is applied to the electronic device, and the specific process may be as follows:

101. and acquiring the current position information of the vehicle and an initial reference track planned in a preset time length.

The vehicle may refer to an unmanned vehicle (egocar), also referred to as an autonomous vehicle, which may be referred to as a self-vehicle for short, wherein the current location information may refer to location information of the vehicle at the current time, such as coordinates of the vehicle at the current time in a ground coordinate system, a lane in which the vehicle is located at the current time, and the like. The initial reference trajectory may refer to a trajectory planned in advance by the unmanned vehicle, and the initial reference trajectory may be planned in a variety of ways, and may be planned according to a requirement in practical application, which is not limited herein.

The preset duration can be set in various ways, for example, flexibly set according to the requirements of practical application, and can also be preset and stored in the electronic device. In addition, the preset duration may be built in the electronic device, or may be saved in the memory and sent to the electronic device, and so on. For example, the preset time period may be set to 5 seconds.

For example, when the vehicle encounters an obstacle during traveling, such as an emergency jam of another vehicle, or needs an emergency avoidance, the traveling trajectory may be optimized by using an iLQR (iterative linear quadratic regulator) algorithm. The iLQR is a track optimization algorithm, and an optimal or sub-optimal solution meeting constraint conditions is obtained by iteratively using the LQR to continuously optimize a target function of a track. The iLQR is suitable for processing complex problems of a nonlinear system, nonlinear constraint and a nonlinear target function, and the aim of simplifying and efficiently solving the complex nonlinear problem is fulfilled by carrying out local linear processing on the system and carrying out quadratic processing on the constraint and the target function. iLQR is generally used to solve a discrete-time finite field trajectory planning problem, and the expression may be as follows:

wherein s.t (Subject to abbreviation) is a constraint,

for the system state vector of step k from the current time,

in order to plan the state of the system at the current time,

and

respectively process cost and final value cost,

and

respectively, a process constraint and a final value constraint. The optimization vector is defined as

N is the maximum number of steps of the trajectory plan, i.e. (e.g., N = 10-20). f is a kinetic equation describing the change of state of the discrete system. For example, setting a system state vector

Control vector

Equation of system dynamics

Described by the following expression:

wherein the content of the first and second substances,

、

is the coordinate of the self-vehicle under the ground coordinate system,

in order to be the speed of the vehicle,

in order to orient the angle (yaw),

and

acceleration and angular velocity, respectively. For example, a planning duration (i.e., a preset duration) T =5 seconds, a planning step number N =20 (i.e., the preset duration may include 20 target time instants), and each time step (time step) is equal to dt = T/N =0.25 seconds. Since the iLQR algorithm cannot directly process the constraint function, constraint information is required to be processed under normal conditions

，

Conversion to cost function (Soft constraint)

By design generationFunction of price

To satisfy the constraint. In this scheme, an optimization problem may be designed for an emergency avoidance function, and the emergency avoidance function may be implemented by using the iLQR, which may be described in detail in the following description. Among them, Ipopt (interior point optimizer), SQP (sequential quadratic programming), and nMPC (nonlinear model predictive control) can also be used for trajectory planning and optimization, but relatively speaking, the calculation load is heavier, and is not favorable for cost control.

102. Determining characteristic constraint information of the driving characteristics of the vehicle in the initial reference track, and calculating the basic cost of the initial reference track based on the characteristic constraint information.

The driving characteristics may refer to parameters of the vehicle during driving, for example, the driving characteristics may include driving energy, driving speed, lane boundary, vehicle acceleration, vehicle yaw rate, and the like. The characteristic constraint information may refer to information that is constrained by a running characteristic of the vehicle, and for example, the characteristic constraint information may include lane speed constraint information (e.g., lane speed limit), boundary constraint information, acceleration constraint information (e.g., maximum acceleration, minimum acceleration), angular velocity constraint information (e.g., maximum yaw rate, minimum yaw rate), and the like. The process cost may include a basic cost and an obstacle cost, where the basic cost represents a cost that the vehicle travels in a preset time period, for example, the cost that the vehicle generally needs to travel in the preset time period may be represented, and the basic cost may include an energy cost, a vehicle speed cost, a boundary cost, an acceleration cost, an angular velocity cost, and the like.

For example, the driving characteristics may include driving energy, the basic cost may include an energy cost, specifically, a preset energy weight may be obtained, energy constraint information of the driving energy of the vehicle in the initial reference trajectory is determined, and the energy cost of the initial reference trajectory is calculated based on the energy constraint information. For example, the specific calculation may be as follows:

wherein the content of the first and second substances,

in order to be the weight, the weight is,

and

directly affecting the smoothness of the trajectory and the smoothness of the control signal, in general

And

it is set to a larger value to increase the degree of trajectory smoothing. For the development of the function of emergency avoidance of obstacles, avoidance by braking rather than overtaking by accelerating is often encouraged, so that the development of the function of emergency avoidance of obstacles is encouraged to adopt a method of avoidance by braking, and overtaking is not always encouraged

May be set in a segment type. For example, it can be set in the following form,

，

，

。

for example, the driving characteristics may include a driving speed, the basic cost may include a speed cost, specifically, the current driving speed and a preset speed weight of the vehicle may be obtained, the target speed of the vehicle after a preset duration is determined, and the speed cost of the initial reference trajectory is calculated based on the current driving speed, the target speed and the preset speed weight of the vehicle. The method for determining the target vehicle speed of the vehicle after the preset time period can be various, for example, the following distance between the vehicle and a guided vehicle, the lane speed limit of the current lane of the vehicle and the state of the guided vehicle can be obtained specifically; and determining the target vehicle speed after the preset time length of the vehicle based on the following distance, the lane speed limit and the guiding vehicle state. The lead car/leader may refer to a nearest environmental vehicle that is present or is about to be present in a target lane in front of the host vehicle during the traveling of the vehicle. The lead vehicle state may refer to the lead vehicle's current system state. For example, the vehicle speed cost may be calculated as follows:

wherein the content of the first and second substances,

in order to be the weight, the weight is,

is the target vehicle speed. Adjustment of

Mainly influences the acceleration and deceleration of the vehicle. The design of the target vehicle speed often needs to consider the following distance, the lane speed limit, the state of the guided vehicle and the like.

Wherein the content of the first and second substances,

in order to limit the speed of the lane,

in order to guide the speed of the vehicle,

the distance between the self vehicle and the guide vehicle,

for the target distance between cars, generally by the distance between cars

Is defined by

，

In order to control the speed of the bicycle,

and second.

For example, the driving characteristics may include lane boundaries, the basic cost may include a boundary cost, and specifically, the lane left boundary and the lane right boundary of the vehicle may be determined according to the current position information of the vehicle; determining boundary constraint information of the vehicle in the initial reference trajectory based on the lane left boundary and the lane right boundary; calculating a boundary cost of the initial reference trajectory based on the boundary constraint information. And the left boundary and the right boundary of the lane define a search range for defining a trajectory plan. For example, it may be assumed that the lane boundaries may be described by polynomial curves, i.e.

，

Then, the boundary cost may include a left boundary cost and a right boundary cost, and the specific calculation may be as follows:

wherein the content of the first and second substances,

、

are coefficients.

And

may be a matrix

，

。

For example, the driving characteristics may include vehicle acceleration, the basic cost may include acceleration cost, and specifically, the maximum acceleration and the minimum acceleration of the vehicle acceleration in the initial reference trajectory may be determined; and calculating the acceleration cost of the initial reference track based on the minimum acceleration and the maximum acceleration. For example, the acceleration constraint may be

The acceleration cost may include a minimum acceleration cost and a maximum acceleration cost, and the specific calculation may be as follows:

wherein the content of the first and second substances,

、

respectively set minimum acceleration and maximum acceleration,

、

as a function of the number of the coefficients,

。

for example, the driving characteristics may include a vehicle yaw rate, the basic costs may include an angular velocity cost, and particularly, a minimum yaw rate and a maximum yaw rate of the vehicle in the initial reference trajectory may be determined; based on the minimum yaw-rate and the maximum yaw-rate, an angular-rate cost of the initial reference trajectory is calculated. For example, the yaw rate constraint may be:

then the angular velocity cost may include a minimum yaw-rate cost and a maximum yaw-rate cost, and the specific calculation may be as follows:

wherein the content of the first and second substances,

、

respectively the set minimum yaw rate and the maximum yaw rate,

、

as a function of the number of the coefficients,

. As used herein with respect to

And

the setting of (a) is more critical, and improper values may affect the feasibility of the planned trajectory, for example, the following manner may be adopted to set

And

：

wherein the content of the first and second substances,

is the set maximum lateral acceleration. Assuming a road adhesion coefficient of

Then, then

Can be set as

Wherein

Is a constant of the acceleration of gravity,

is a settable coefficient.

103. At least one target obstacle of the vehicle is identified based on the current position information, and an obstacle cost of the initial reference trajectory is calculated according to the initial reference trajectory and the predicted travel trajectory of each target obstacle.

The target obstacle may refer to a terrain, a feature, other equipment, etc. that may retard or prevent the vehicle from moving. The target obstacle may be stationary or dynamic, and may refer to, for example, a vehicle that can retard or prevent the unmanned vehicle from traveling normally while the unmanned vehicle is traveling. The predicted travel trajectory of the target obstacle may refer to a trajectory that the target obstacle is predicted to travel according to a current travel state of the target obstacle, such as a trajectory that the target obstacle is predicted to travel within a preset time period. The predicted travel path of the target obstacle may be predicted in a variety of ways, for example, the behavior of the target obstacle may be preset by using a neural network to generate the predicted travel path of the target obstacle, and the prediction method may be selected according to actual requirements, which is not limited herein. Wherein the obstacle penalty characterizes a penalty the vehicle paid to avoid the target obstacle.

For example, the current lane in which the vehicle is located may be specifically determined according to the current position information; respectively acquiring adjacent vehicles meeting a preset distance in a current lane and adjacent lanes; calculating a time of collision of the vehicle with the neighboring vehicle; and determining the adjacent vehicle with the collision time meeting the preset threshold value as the target obstacle of the vehicle. Wherein, the adjacent lanes may refer to a left lane and a right lane of a current lane where the vehicle is located.

For example, a vehicle may be present in front of and behind the current lane and the two adjacent left and right lanes, which are closest to the host vehicle, that is, 6 vehicles may be considered at the same time. Of the 6 vehicles, the lead vehicle is first selected to place the target obstacle set

For the remaining 5 vehicles, the Time To Collision (TTC) between the obstacle and the vehicle can be determined one by one, and when the time to collision is less than a threshold (for example, 2 seconds), the vehicle is put into the vehicle

Otherwise, the vehicle is ignored. For example, TTC may be calculated as follows:

wherein s and v respectively represent the longitudinal position and the speed of the front vehicle and the rear vehicle in a Frenet coordinate system. The Frenet coordinate system may refer to a coordinate system established by using a reference point on a curve as an origin of coordinates, and using a tangent, a normal line and a secondary normal line as three coordinate systems.

Among these, the Frenet coordinate system is a way to represent road locations in a more intuitive way than the traditional x, y Cartesian coordinates. With Frenet coordinates, the variables s and d can be used to describe the location of the vehicle on the roadway. The s-coordinate represents a distance along the road (also referred to as a longitudinal displacement) and the d-coordinate represents a left-right position on the road (also referred to as a lateral displacement).

For example, a collision polygon of the vehicle and the target obstacle at the at least one target time may be specifically constructed based on the initial reference trajectory and the predicted travel trajectory of the target obstacle; calculating a closest distance of the vehicle to the collision polygon at the at least one target time; calculating an instantaneous collision cost of the vehicle with the target obstacle based on the closest distance; and calculating the obstacle cost of the initial reference track according to the instantaneous collision cost.

The collision polygon of the vehicle and the target obstacle at the at least one target moment can be constructed in various ways, for example, the shape information of the vehicle at the target moment can be determined based on the initial reference track of the vehicle; determining shape information of the target obstacle at a target time based on the predicted travel track of the target obstacle; -calculating a minkowski sum of said vehicle and said target obstacle at a target moment in time using shape information of said vehicle and shape information of said target obstacle, -constructing a collision polygon from said minkowski sum. The shape information may refer to a shape of the whole object, for example, an unmanned vehicle may be considered as a rectangle.

The method for calculating the closest distance between the vehicle and the collision polygon at the at least one target time may be various, for example, a target vertex at which the vehicle is closest to the collision polygon at the target time may be specifically determined; dividing the plane space of the vehicle into at least one area by taking the target vertex as a center; and determining a target area where the vehicle is located, and calculating the closest distance between the vehicle and the collision polygon based on the distance from the midpoint of the vehicle to the target vertex and the target area. The midpoint of the vehicle may refer to a point in the center of the vehicle, for example, when the vehicle is regarded as a rectangle, the midpoint thereof may be an intersection point of diagonals of the rectangle.

For example, the specific expression may be as follows:

wherein the content of the first and second substances,

for each obstacle cost function, Ω is the set of target obstacles. For each obstacle, the obstacle cost function needs to be evaluated according to the current planned trajectory of the vehicle and the predicted trajectory of the obstacle. For example,

the expression of (a) may be as follows:

wherein, the preset time domain (i.e. the preset duration) may be T =5 seconds,

and presetting the instantaneous collision cost of the vehicle and the barrier at the moment t. For example, the instantaneous cost can be calculated by using a constructed collision polygon (e.g., a collision octagon)

. The collision octagon is a Minkowski sum (Minkowski sum) of the bicycle shape (approximately rectangular) and the obstacle shape (approximately rectangular).

For example, given a collision octagon at time t, we only need to calculate the planned position of the own vehicle at time t

The distance from the colliding octagon defines the cost function. The minimum clearance required to be maintained for avoiding collision with an obstacle is assumed to be

Then the cost function for the obstacle may be:

wherein the content of the first and second substances,

、

as a function of the number of the coefficients,

the closest distance from the vehicle to the octagon at time t.

The above steps set the process cost function

For each sub-term of (a), the expression of the process cost may be as follows:

。

104. and calculating the final value cost of the initial reference track according to the current system state and the target system state of the vehicle, wherein the target system state is the state which the vehicle is expected to reach after a preset time length.

The current system state may refer to the current state of the vehicle, for example, as seen in step 101, the current system state of the vehicle may include the current coordinates of the vehicle in the ground coordinate system, the current driving speed, the current orientation angle, and the like. And the final value cost represents the cost of the vehicle at the last target moment of the preset time length. For example, the specific expression of the final cost may be as follows:

wherein the content of the first and second substances,

in order to be the weight, the weight is,

in order to be the target system state,

the matrix can be set according to actual conditions. The target system state may refer to a state that the vehicle is expected to reach after a preset period of time, for example, the target system state may include target coordinates, a target speed, a target heading angle, and the like that the vehicle reaches after the preset period of time.

105. Determining a global cost of the initial reference trajectory based on the base cost, the obstacle cost, and the final value cost.

The global cost may refer to all costs to be paid by the vehicle during driving within a preset time period, for example, the global cost may include a basic cost, an obstacle cost, a final cost, and the like, that is, a process cost and a final cost.

For example, a process cost of the initial reference trajectory may be determined based on the basic cost and the obstacle cost, a global cost of an objective function may be determined based on the process cost and the final value cost, and the objective function of the initial reference trajectory may be optimized based on the global cost to obtain an optimal solution that satisfies a constraint condition.

106. And updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

For example, the preset duration may include at least one target time, and specifically, if the global cost does not satisfy the convergence condition, the optimal control rate of each target time in the initial reference trajectory may be calculated in a reverse direction from the last target time of the preset duration; determining target track points according to the optimal control rate of each target moment; and updating the initial reference track based on the target track point until the global cost meets a convergence condition to obtain an updated reference track.

For example, specifically, if the global cost does not satisfy the convergence condition, the optimal control rate of each target time in the initial reference trajectory may be reversely calculated by using an objective function from the last target time of a preset duration to the current time; and according to the optimal control rate of each target moment, carrying out forward gradual iteration from the current moment to the last target moment with preset duration to obtain an optimized reference track, calculating the global cost of the optimized reference track, and controlling the vehicle to run by taking the optimized reference track as an updated reference track if the global cost of the optimized reference track meets the convergence condition.

For example, an initial reference trajectory at the current time may be given, a reverse transfer process may be performed by using the iLQR, and starting from the last target time with a preset duration, the objective function may be optimized

And calculating the optimal control rate of each step from the T-T/N to 0. (e.g., an optimal control rate of 4.75 seconds to 0 seconds may be calculated, i.e., the first target time may be 0 seconds, the second target time may be 0.25 seconds, and the … … last target time may be 4.75 seconds). Then, a forward transfer process is carried out again, starting from an initial state (namely the current time), the optimized track can be obtained by gradually iterating forward to the T time by using the optimal control rate obtained by the reverse process, the global cost is calculated, the processes can be continuously repeated, the optimized track is used as a new reference track, and the iLQR iterative optimization is carried out for a plurality of times until the limit of iteration times or the objective function is reached

And converging (namely the global cost meets the convergence condition), and obtaining the optimal optimized track, namely the updated reference track.

As can be seen from the above, the present embodiment may obtain current position information of a vehicle and an initial reference trajectory planned within a preset time period, then determine feature constraint information of a driving feature of the vehicle in the initial reference trajectory, calculate a basic cost of the initial reference trajectory based on the feature constraint information, identify at least one target obstacle of the vehicle based on the current position information, calculate an obstacle cost of the initial reference trajectory according to the initial reference trajectory and a predicted driving trajectory of each target obstacle, calculate a final cost of the initial reference trajectory according to a current system state and a target system state of the vehicle, where the target system state is a state that the vehicle reaches a later stage within the preset time period, and then determine a global cost of the initial reference trajectory based on the basic cost, the obstacle cost, and the final cost, and then, updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track. According to the scheme, through optimization of the energy, the speed, the boundary, the acceleration, the yaw rate, the barrier and the like of the unmanned vehicle, horizontal and longitudinal combined optimization of the planned trajectory of the unmanned vehicle in the space-time field is achieved, the stability, the feasibility, the comfort and the safety of the trajectory planning are better met, the lane space is fully utilized to strive for the braking time, meanwhile, the cost control requirement is met, the unmanned vehicle can flexibly avoid the barrier in an emergency state, and the driving safety of the unmanned vehicle is greatly improved.

The method described in the previous embodiment is further detailed by way of example.

In this embodiment, the vehicle trajectory optimization device will be described by taking an example in which the vehicle trajectory optimization device is specifically integrated in an electronic device.

First, a cost function for satisfying emergency avoidance may be designed.

For example, when the vehicle encounters an obstacle in the driving process, such as an emergency jam of another vehicle, or needs an emergency avoidance, the optimization of the driving track can be realized by using the iLQR algorithm. The iLQR is a track optimization algorithm, and an optimal or sub-optimal solution meeting constraint conditions is obtained by iteratively using the LQR to continuously optimize a target function of a track. The iLQR is suitable for processing complex problems of a nonlinear system, nonlinear constraint and a nonlinear target function, and the aim of simplifying and efficiently solving the complex nonlinear problem is fulfilled by carrying out local linear processing on the system and carrying out quadratic processing on the constraint and the target function. iLQR is generally used to solve a discrete-time finite field trajectory planning problem, and the expression may be as follows:

wherein, s.t. is a constraint condition,

for the system state vector of step k from the current time,

in order to plan the state of the system at the current time,

and

respectively process cost and final value cost,

and

And N is the maximum step number of the trajectory planning (i.e., N = 10-20). f is a kinetic equation describing the change of state of the discrete system. For example, a system state vector may be set

Control vector

Equation of system dynamics

Described by the following expression:

wherein the content of the first and second substances,

、

is the coordinate of the self-vehicle under the ground coordinate system,

in order to be the speed of the vehicle,

in order to orient the angle (yaw),

and

acceleration and angular velocity, respectively. For example, a planning duration (i.e., a preset duration) T =5 seconds, a planning step number N =20, and each time step (time step) is dt = T/N =0.25 seconds. Since the iLQR algorithm cannot directly process the constraint function, constraint information is required to be processed under normal conditions

，

Conversion to cost function (Soft constraint)

By designing a cost function

To satisfy the constraint. The emergency avoidance function can be designed and optimized, and the iLQR is used to realize the emergency avoidance function, for example, the following cost functions can be designed to meet the requirement of the emergency avoidance function.

1. Energy is lost. I.e. the energy cost can be designed as:

，

for weighting, designing

And

And

it is designed to be larger to increase the degree of trajectory smoothing. For the emergency obstacle avoidance function, avoidance by braking rather than overtaking by acceleration is often encouraged, and therefore

It can be designed in a segmented manner. For example, it can be designed in the following form,

，

，

。

2. the target vehicle speed. Namely, the vehicle speed cost can be designed as:

，

in order to be the weight, the weight is,

is the target vehicle speed. Adjustment of

Mainly influences the acceleration and deceleration of the vehicle. Target vehicle speedThe design of (2) usually needs to consider the following distance, the lane speed limit, the guiding vehicle state and the like.

Wherein the content of the first and second substances,

in order to limit the speed of the lane,

in order to guide the speed of the vehicle,

the distance between the self vehicle and the guide vehicle,

for the target distance between cars, generally by the distance between cars

Is defined by

，

In order to control the speed of the bicycle,

and second.

3. And (4) restraining a lane boundary. And the left side and the right side of the lane define a search range for defining a trajectory plan. It is assumed that the lane boundaries can be described by polynomial curves (local coordinate system as shown in fig. 2 a), i.e.

，

The boundary cost may include a left boundary cost and a right boundary costBoundary cost, left boundary cost

And right boundary cost

The design of (2) may be as follows:

wherein the content of the first and second substances,

、

are coefficients. Matrix array

，

。

4. And (3) acceleration restraint:

the acceleration cost may include a minimum acceleration cost and a maximum acceleration cost, and the minimum acceleration cost may be designed as:

the maximum acceleration cost can be designed as:

wherein

、

Respectively designed minimum acceleration and maximum acceleration,

、

as a function of the number of the coefficients,

。

5. and (3) restricting the yaw velocity:

the angular velocity cost may include a minimum yaw-rate cost and a maximum yaw-rate cost, and the minimum yaw-rate cost may be designed as:

the maximum yaw rate cost can be designed as:

wherein

，

Respectively designed minimum acceleration and maximum acceleration,

，

as a function of the number of the coefficients,

. As used herein with respect to

And

the design of (2) is more critical, and improper value can influence the feasibility of planning the orbit, and the scheme can be designed in the following mode

And

：

wherein the content of the first and second substances,

to design maximum lateral acceleration. Assuming a road adhesion coefficient of

Then, then

Can be designed as

Wherein

Is a constant of the acceleration of gravity,

are programmable coefficients.

6. Disorder(s)The object constraint, i.e. the obstacle cost, can be designed as:

. Handling obstacle constraints involves two problems: a. selection of obstacles, i.e. set of target obstacles

The definition of (1); b. cost per obstacle function

The design of (3). These two steps are explained below:

a. and selecting a target obstacle. For example, as shown by the ellipse in fig. 2b, only one vehicle before and after the current lane and the two adjacent lanes closest to the own vehicle are considered, that is, at most 6 vehicles are considered simultaneously. Of these 6 vehicles, the lead vehicle (e.g., vehicle 01 in front of vehicle 00 in FIG. 2 b) is first selected for placement

For the remaining 5 vehicles, the collision time (TTC) between the obstacle and the vehicle needs to be determined one by one, and when the collision time is less than a certain threshold (for example, 2 seconds), the vehicle is put into the vehicle

Otherwise, the vehicle is ignored. TTC can be calculated as follows:

wherein s and v respectively represent the longitudinal position and the speed of the front vehicle and the rear vehicle in a Frenet coordinate system.

b. Cost function of obstacles

. For each obstacle, the patent needs to predict the obstacle according to the current planned track of the self vehicleThe cost function is evaluated in the form of an evaluation,

wherein the time domain T =5 seconds,

the instantaneous collision cost of the own vehicle and the obstacle at the moment of aiming t is predicted. This patent requires the design to use the collision octagon to calculate the instantaneous cost

. The collision octagon is a Minkowski sum with the shape of the own vehicle (approximate rectangle) and the shape of the obstacle (approximate rectangle), as shown in FIG. 2c, for example, given the collision octagon at time t (i.e. a certain target time), only the planned position of the own vehicle at time t needs to be calculated

Then the cost function for the obstacle may be:

wherein the content of the first and second substances,

、

as a function of the number of the coefficients,

the closest distance from the vehicle to the octagon at time t.

The calculation of (c) can be seen with reference to figure 2d,assuming that the relationship between the own vehicle position and the obstacle collision octagon at time t is shown in fig. 2d, where P denotes the own vehicle position (i.e., the midpoint of the vehicle), O denotes the vertex of the octagon closest to the own vehicle (i.e., the target vertex), and the plane space may be divided into 5 regions with O as the center: I. II, III, IV and cone region. Zone III and zone IV consisting of

Defines the boundary. Suppose that

，

Is a unit vector which is vertical to the outside of the octagon, then

Can be given by:

wherein the content of the first and second substances,

indicating the length of the line segment. For example, as shown in FIG. 2d, if the target area where the vehicle is located is the cone area, then

Can be that

。

The above steps 1-6 design the process cost function

Each sub-item of (1):

。

7. in addition, a final value error needs to be designed to ensure the validity of the end state of the planned trajectory, and generally only the speed and the direction of the end state are concerned.

The final error may be calculated as follows:

wherein the content of the first and second substances,

in order to be the weight, the weight is,

is the target state.

And (II) the running track of the vehicle can be optimized by utilizing the designed objective function.

As shown in fig. 2e, a specific process of the vehicle trajectory optimization method may be as follows:

201. the electronic equipment acquires current position information of the vehicle and an initial reference track planned within a preset time length.

For example, the electronic device may acquire coordinates of the vehicle at the current time in a ground coordinate system, a lane in which the vehicle is located at the current time, and the like. The electronic device may also obtain an initial reference trajectory that has been pre-planned within a preset time period in the future.

The preset duration can be set in various ways, for example, flexibly set according to the requirements of practical application, and can also be preset and stored in the electronic device. In addition, the preset duration may be built in the electronic device, or may be saved in the memory and sent to the electronic device, and so on.

202. The electronic device calculates an energy cost of the initial reference trajectory.

For example, the electronic device may specifically obtain a preset energy weight, determine energy constraint information of energy of the vehicle traveling in the initial reference trajectory, and calculate an energy cost of the initial reference trajectory based on the energy constraint information.

203. The electronic device calculates a vehicle speed cost of the initial reference trajectory.

For example, the electronic device may specifically obtain a current running vehicle speed of the vehicle and a preset vehicle speed weight, determine a target vehicle speed of the vehicle after a preset time period, and calculate a vehicle speed cost of the initial reference trajectory based on the current running vehicle speed of the vehicle, the target vehicle speed and the preset vehicle speed weight.

For example, the electronic device may specifically determine to acquire a following distance between the vehicle and a guided vehicle, a lane speed limit of a current lane of the vehicle, and a state of the guided vehicle; and determining the target vehicle speed after the preset time length of the vehicle based on the following distance, the lane speed limit and the guiding vehicle state.

204. The electronic device calculates a boundary cost of the initial reference trajectory.

For example, the electronic device may specifically determine a lane left boundary and a lane right boundary of the vehicle according to the current position information of the vehicle; determining boundary constraint information of the vehicle in the initial reference trajectory based on the lane left boundary and the lane right boundary; calculating a boundary cost of the initial reference trajectory based on the boundary constraint information.

205. The electronic device calculates an acceleration cost of the initial reference trajectory.

For example, the electronic device may specifically determine a maximum acceleration and a minimum acceleration of the vehicle acceleration in said initial reference trajectory; calculating an acceleration cost of the initial reference trajectory based on the minimum acceleration and the maximum acceleration.

206. The electronic device calculates an angular velocity cost of the initial reference trajectory.

For example, the electronic device may specifically determine a minimum yaw rate and a maximum yaw rate of a vehicle yaw rate of the vehicle in the initial reference trajectory; an angular velocity cost of the initial reference trajectory is calculated based on the minimum yaw-rate and the maximum yaw-rate.

207. The electronic device calculates an obstacle cost of the initial reference trajectory.

For example, the electronic device may specifically determine, according to the current location information, a current lane in which the vehicle is located; respectively acquiring adjacent vehicles meeting a preset distance in a current lane and adjacent lanes; calculating a time of collision of the vehicle with the neighboring vehicle; and determining the adjacent vehicle with the collision time meeting the preset threshold value as the target obstacle of the vehicle.

For example, the electronic device may specifically construct a collision polygon of the vehicle and the target obstacle at the at least one target time based on the initial reference trajectory and the predicted travel trajectory of the target obstacle; calculating a closest distance of the vehicle to the collision polygon at the at least one target time; calculating an instantaneous collision cost of the vehicle with the target obstacle based on the closest distance; and calculating the obstacle cost of the initial reference track according to the instantaneous collision cost.

For example, the electronic device may specifically determine shape information of the vehicle at a target time based on an initial reference trajectory of the vehicle; determining shape information of the target obstacle at a target time based on the predicted travel track of the target obstacle; -calculating a minkowski sum of said vehicle and said target obstacle at a target moment in time using shape information of said vehicle and shape information of said target obstacle, -constructing a collision polygon from said minkowski sum.

For example, the electronic device may specifically determine a target vertex at which the vehicle is closest to the collision polygon at the target time; dividing the plane space of the vehicle into at least one area by taking the target vertex as a center; and determining a target area where the vehicle is located, and calculating the closest distance between the vehicle and the collision polygon based on the distance from the midpoint of the vehicle to the target vertex and the target area.

208. The electronic device calculates a final cost for the initial reference trajectory.

For example, the electronic device may specifically calculate the final cost of the initial reference trajectory based on the following expression:

wherein the content of the first and second substances,

in order to be the weight, the weight is,

in order to be the target system state,

the matrix can be set according to actual conditions. For example, the target system state may include the vehicle reaching a target coordinate, a target speed, a target heading angle, and the like after a preset period of time.

209. And the electronic equipment determines the global cost of the initial reference track based on the energy cost, the vehicle speed cost, the boundary cost, the acceleration cost, the angular speed cost, the obstacle cost and the final value cost.

For example, the electronic device may specifically determine a process cost based on the energy cost, the vehicle speed cost, the boundary cost, the acceleration cost, the angular velocity cost, and the obstacle cost, determine a global cost of the objective function based on the process cost and the final value cost, and optimize the objective function defining the initial reference trajectory based on the global cost to obtain an optimal solution satisfying the constraint condition.

210. And the electronic equipment updates the initial reference track by using the global cost to obtain an updated reference track, and controls the vehicle to run according to the updated reference track.

For example, as shown in FIG. 2f, given the initial reference trajectory at the current time

，

The dotted line is the boundary of the initial reference track, the iLQR firstly carries out a reverse transfer process, and then

Starting from an optimization objective function

And calculating the optimal control rate of each step from the T-T/N to 0. Then, a forward transfer process is performed again from the initial state

Starting from this, the optimal control rate obtained by using the reverse process is gradually iterated forward to time T, so as to obtain an optimized trajectory (a 10 solid line in fig. 2 f), and calculate the global cost. The above processes can be repeated continuously, the optimized track is used as a new reference track, and a plurality of times of iLQR iterative optimization are carried out until the limit of the iterative times or the objective function is reached

And converging to obtain an optimal optimized track (namely the global cost meets a convergence condition), namely the updated reference track. The specific implementation process can be as shown in fig. 2 g.

According to the scheme, unmanned automobiles can be driven at the level of L2.5-L4 and above, and the vehicle jam and irregular driving behaviors in the emergency environment can be avoided. According to the scheme, a large number of real vehicle tests are carried out on a simulation environment, g7 (Beijing new high speed), four-ring and other high speed roads, so that collision behaviors can be effectively avoided.

In order to better implement the method, correspondingly, the embodiment of the present application further provides a vehicle trajectory optimization device, which may be specifically integrated in an electronic device, where the electronic device may be a server or a terminal.

For example, as shown in fig. 3, the vehicle trajectory optimization apparatus may include an acquisition unit 301, a first calculation unit 302, a second calculation unit 303, a third calculation unit 304, a determination unit 305, and an update unit 306, as follows:

an obtaining unit 301, configured to obtain current position information of a vehicle and an initial reference trajectory planned within a preset duration;

a first calculating unit 302, configured to determine feature constraint information of a driving feature of the vehicle in the initial reference trajectory, and calculate a basic cost of the initial reference trajectory based on the feature constraint information;

a second calculating unit 303, configured to identify at least one target obstacle of the vehicle based on the current position information, and calculate an obstacle cost of the initial reference trajectory according to the initial reference trajectory and a predicted travel trajectory of each target obstacle;

a third calculating unit 304, configured to calculate a final value cost of the initial reference trajectory according to a current system state and a target system state of the vehicle, where the target system state is a state that the vehicle is expected to reach after a preset duration;

a determining unit 305, configured to determine a global cost of the initial reference trajectory based on the basic cost, the obstacle cost, and the final value cost;

and the updating unit 306 is configured to update the initial reference trajectory by using the global cost to obtain an updated reference trajectory, and control the vehicle to run according to the updated reference trajectory.

Optionally, in some embodiments, the second calculating unit 303 may include an identifying subunit, as follows:

Optionally, in some embodiments, the preset time duration includes at least one target time, and the second calculating unit 303 may include a calculating subunit, as follows:

Optionally, in some embodiments, the driving characteristics include driving energy, the basic cost includes energy cost, and the first calculating unit 302 may be specifically configured to obtain a preset energy weight and determine energy constraint information of the driving energy of the vehicle in the initial reference trajectory; an energy cost of the initial reference trajectory is calculated based on the energy constraint information.

Optionally, in some embodiments, the driving characteristics include a driving speed, the basic cost includes a speed cost, and the first calculating unit 302 may be specifically configured to obtain a current driving speed of the vehicle and a preset speed weight; determining a target vehicle speed of the vehicle after a preset time; and calculating the vehicle speed cost of the initial reference track based on the current running vehicle speed of the vehicle, the target vehicle speed and a preset vehicle speed weight.

Optionally, in some embodiments, the first calculating unit 302 may be specifically configured to obtain a following distance between the vehicle and a guided vehicle, a lane speed limit of a current lane of the vehicle, and a state of the guided vehicle; and determining the target vehicle speed after the preset time length of the vehicle based on the following distance, the lane speed limit and the guiding vehicle state.

Optionally, in some embodiments, the driving characteristics include lane boundaries, the basic cost includes a boundary cost, and the first calculating unit 302 may be specifically configured to determine a lane left boundary and a lane right boundary of the vehicle according to the current position information of the vehicle; determining boundary constraint information of the vehicle in the initial reference trajectory based on the lane left boundary and the lane right boundary; calculating a boundary cost of the initial reference trajectory based on the boundary constraint information.

Optionally, in some embodiments, the driving characteristics include vehicle acceleration, the basic cost includes an acceleration cost, and the first calculation unit 302 may be specifically configured to determine a maximum acceleration and a minimum acceleration of the vehicle acceleration in the initial reference trajectory; calculating an acceleration cost of the initial reference trajectory based on the minimum acceleration and the maximum acceleration.

Optionally, in some embodiments, the driving characteristics include a vehicle yaw rate, the basic cost includes a cost of an angular velocity, and the first computing unit 302 may be specifically configured to determine a minimum yaw rate and a maximum yaw rate of the vehicle in the initial reference trajectory; an angular velocity cost of the initial reference trajectory is calculated based on the minimum yaw-rate and the maximum yaw-rate.

Optionally, in some embodiments, the updating unit 306 may be specifically configured to, if the global cost does not satisfy the convergence condition, reversely calculate, from the last target time of a preset duration, an optimal control rate of each target time in the initial reference trajectory; determining target track points according to the optimal control rate of each target moment; and updating the initial reference track based on the target track point until the global cost meets a convergence condition to obtain an updated reference track.

In a specific implementation, the above units may be implemented as independent entities, or may be combined arbitrarily to be implemented as the same or several entities, and the specific implementation of the above units may refer to the foregoing method embodiments, which are not described herein again.

As can be seen from the above, in the present embodiment, the obtaining unit 301 obtains the current position information of the vehicle and the initial reference trajectory planned in the preset time period, then the first calculating unit 302 determines the characteristic constraint information of the driving characteristics of the vehicle in the initial reference trajectory, and calculates the basic cost of the initial reference trajectory based on the characteristic constraint information, the second calculating unit 303 identifies at least one target obstacle of the vehicle based on the current position information, and calculates the obstacle cost of the initial reference trajectory according to the initial reference trajectory and the predicted driving trajectory of each target obstacle, the third calculating unit 304 calculates the final cost of the initial reference trajectory according to the current system state and the target system state of the vehicle, and the target system state is the state that the vehicle is expected to reach after the preset time period, then, the determining unit 305 determines a global cost of the initial reference trajectory based on the basic cost, the obstacle cost, and the final value cost, and then the updating unit 306 updates the initial reference trajectory by using the global cost to obtain an updated reference trajectory, and controls the vehicle to travel according to the updated reference trajectory. According to the scheme, through optimization of the energy, the speed, the boundary, the acceleration, the yaw rate, the barrier and the like of the unmanned vehicle, horizontal and longitudinal combined optimization of the planned trajectory of the unmanned vehicle in the space-time field is achieved, the stability, the feasibility, the comfort and the safety of the trajectory planning are better met, the lane space is fully utilized to strive for the braking time, meanwhile, the cost control requirement is met, the unmanned vehicle can flexibly avoid the barrier in an emergency state, and the driving safety of the unmanned vehicle is greatly improved.

In addition, an electronic device according to an embodiment of the present application is further provided, as shown in fig. 4, which shows a schematic structural diagram of the electronic device according to an embodiment of the present application, and specifically:

the electronic device may include components such as a processor 401 of one or more processing cores, memory 402 of one or more computer-readable storage media, a power supply 403, and an input unit 404. Those skilled in the art will appreciate that the electronic device configuration shown in fig. 4 does not constitute a limitation of the electronic device and may include more or fewer components than those shown, or some components may be combined, or a different arrangement of components. Wherein:

the processor 401 is a control center of the electronic device, connects various parts of the whole electronic device by various interfaces and lines, performs various functions of the electronic device and processes data by running or executing software programs and/or modules stored in the memory 402 and calling data stored in the memory 402, thereby performing overall monitoring of the electronic device. Optionally, processor 401 may include one or more processing cores; preferably, the processor 401 may integrate an application processor, which mainly handles operating systems, user interfaces, application programs, etc., and a modem processor, which mainly handles wireless communications. It will be appreciated that the modem processor described above may not be integrated into the processor 401.

The memory 402 may be used to store software programs and modules, and the processor 401 executes various functional applications and data processing by operating the software programs and modules stored in the memory 402. The memory 402 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application program required by at least one function (such as a sound playing function, an image playing function, etc.), and the like; the storage data area may store data created according to use of the electronic device, and the like. Further, the memory 402 may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other volatile solid state storage device. Accordingly, the memory 402 may also include a memory controller to provide the processor 401 access to the memory 402.

The electronic device further comprises a power supply 403 for supplying power to the various components, and preferably, the power supply 403 is logically connected to the processor 401 through a power management system, so that functions of managing charging, discharging, and power consumption are realized through the power management system. The power supply 403 may also include any component of one or more dc or ac power sources, recharging systems, power failure detection circuitry, power converters or inverters, power status indicators, and the like.

The electronic device may further include an input unit 404, and the input unit 404 may be used to receive input numeric or character information and generate keyboard, mouse, joystick, optical or trackball signal inputs related to user settings and function control.

Although not shown, the electronic device may further include a display unit and the like, which are not described in detail herein. Specifically, in this embodiment, the processor 401 in the electronic device loads the executable file corresponding to the process of one or more application programs into the memory 402 according to the following instructions, and the processor 401 runs the application program stored in the memory 402, thereby implementing various functions as follows:

acquiring current position information of a vehicle and an initial reference track planned in a preset time period, then determining characteristic constraint information of running characteristics of the vehicle in the initial reference track, calculating a basic cost of the initial reference track based on the characteristic constraint information, identifying at least one target obstacle of the vehicle based on the current position information, calculating an obstacle cost of the initial reference track according to the initial reference track and a predicted running track of each target obstacle, calculating a final value cost of the initial reference track according to a current system state and a target system state of the vehicle, wherein the target system state is a state expected to be reached by the vehicle in the later period of the preset time period, and then determining a global cost of the initial reference track based on the basic cost, the obstacle cost and the final value cost, and then, updating the initial reference track by using the global cost to obtain an updated reference track, and controlling the vehicle to run according to the updated reference track.

The above operations can be implemented in the foregoing embodiments, and are not described in detail herein.

It will be understood by those skilled in the art that all or part of the steps of the methods of the above embodiments may be performed by instructions or by associated hardware controlled by the instructions, which may be stored in a computer readable storage medium and loaded and executed by a processor.

To this end, the present application further provides a storage medium, in which a plurality of instructions are stored, where the instructions can be loaded by a processor to execute the steps in any one of the vehicle trajectory optimization methods provided in the embodiments of the present application. For example, the instructions may perform the steps of:

Wherein the storage medium may include: read Only Memory (ROM), Random Access Memory (RAM), magnetic or optical disks, and the like.

Since the instructions stored in the storage medium can execute the steps in any vehicle trajectory optimization method provided in the embodiments of the present application, the beneficial effects that can be achieved by any vehicle trajectory optimization method provided in the embodiments of the present application can be achieved, and detailed descriptions are omitted here for the detailed description.

The embodiment of the present application further provides a vehicle navigation device, as shown in fig. 5, which shows a schematic structural diagram of the vehicle navigation device according to the embodiment of the present application, specifically:

the vehicular navigation apparatus may include a positioning device 501, a map generating device 502, and a navigation device 503, and those skilled in the art will appreciate that the vehicular navigation apparatus structure shown in fig. 5 does not constitute a limitation of the vehicular navigation apparatus, and may include more or less components than those shown, or combine some components, or a different arrangement of components. Wherein:

the positioning device 501 provides the vehicular navigation apparatus with real-time position information of the vehicle using the vehicular navigation apparatus.

The map generating device 502 provides the three-dimensional map information of the road where the vehicle is located for the vehicle navigation device, and acquires the position information and the current road condition information of the vehicle using the vehicle navigation device in the three-dimensional map by combining the real-time position information of the vehicle.

The navigation device 503 may plan a route for the vehicle according to the destination input by the vehicle and the real-time location information of the vehicle and generate a three-dimensional map, generate a corresponding control instruction according to the route planning information, and control the unmanned vehicle to reach the set destination according to the planned route according to the control instruction.

The present application further provides an unmanned vehicle, as shown in fig. 6, which shows a schematic structural diagram of the unmanned vehicle according to the present application, specifically:

the unmanned vehicle may include a drive system 601, a control system 602, and a navigation system 603. those skilled in the art will appreciate that the electronic configuration shown in fig. 6 does not constitute a limitation of the unmanned vehicle and may include more or fewer components than shown, or some components in combination, or a different arrangement of components. Wherein:

the driving system 601 is a power source of the unmanned vehicle, and can extract driving force for the unmanned vehicle to realize driving functions of the unmanned vehicle such as forward movement, backward movement, stopping and the like. The drive system may include an engine, transmission, wheels, and the like.

The control system 602 is a control core of the unmanned vehicle, and the control system can control the unmanned vehicle to start, stop and steer, and can also control the unmanned vehicle to run according to a planned path in real time according to changes of an external environment. The control system may include an unmanned vehicle control device.

The navigation system 603 may plan a route for vehicle travel according to destination information of the vehicle, generate a control command for the route according to the planned route, input the control command to the control system 602, and drive the driving system 601 based on the control system 602, so that the unmanned vehicle reaches the destination according to the planned route.

The vehicle trajectory optimization method, the vehicle trajectory optimization device, the electronic device, and the storage medium provided in the embodiments of the present application are described in detail above, and specific examples are applied herein to illustrate the principles and implementations of the present application, and the description of the embodiments above is only used to help understand the method and the core ideas of the present application; meanwhile, for those skilled in the art, according to the idea of the present application, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present application.

Claims

1. A vehicle trajectory optimization method, comprising:

2. The method of claim 1, wherein the identifying at least one target obstacle for the vehicle based on the current location information comprises:

determining a current lane where the vehicle is located according to the current position information;

respectively acquiring adjacent vehicles meeting a preset distance in a current lane and adjacent lanes;

calculating a time of collision of the vehicle with the neighboring vehicle;

and determining the adjacent vehicle with the collision time meeting the preset threshold value as the target obstacle of the vehicle.

3. The method of claim 1, wherein the preset duration comprises at least one target time, and wherein calculating the obstacle cost for the initial reference trajectory based on the initial reference trajectory and the predicted travel trajectory for each target obstacle comprises:

constructing a collision polygon of the vehicle and the target obstacle at the at least one target moment based on the initial reference trajectory and a predicted travel trajectory of the target obstacle;

calculating a closest distance of the vehicle to the collision polygon at the at least one target time;

calculating an instantaneous collision cost of the vehicle with the target obstacle based on the closest distance;

and calculating the obstacle cost of the initial reference track according to the instantaneous collision cost.

4. The method of claim 3, wherein said constructing a collision polygon of the vehicle with the target obstacle at the at least one target moment based on the initial reference trajectory and the predicted travel trajectory of the target obstacle comprises:

determining shape information of the vehicle at a target moment based on an initial reference trajectory of the vehicle;

determining shape information of the target obstacle at a target time based on the predicted travel track of the target obstacle;

-calculating a minkowski sum of said vehicle and said target obstacle at a target moment in time using shape information of said vehicle and shape information of said target obstacle, -constructing a collision polygon from said minkowski sum.

5. The method of claim 3, wherein said calculating a closest distance of the vehicle to the collision polygon at the at least one target time comprises:

determining a target vertex of the vehicle closest to the collision polygon at a target moment;

dividing the plane space of the vehicle into at least one area by taking the target vertex as a center;

and determining a target area where the vehicle is located, and calculating the closest distance between the vehicle and the collision polygon based on the distance from the midpoint of the vehicle to the target vertex and the target area.

6. The method according to any one of claims 1 to 5, wherein the travel characteristic includes travel energy, the basic cost includes an energy cost, and the determining feature constraint information of the travel characteristic of the vehicle in the initial reference trajectory and calculating the basic cost of the initial reference trajectory based on the feature constraint information includes:

acquiring a preset energy weight, and determining energy constraint information of the running energy of the vehicle in the initial reference track;

an energy cost of the initial reference trajectory is calculated based on the energy constraint information.

7. The method according to any one of claims 1 to 5, wherein the running characteristic includes a running vehicle speed, the basic cost includes a vehicle speed cost, the determining feature constraint information of the running characteristic of the vehicle in the initial reference trajectory, and calculating the basic cost of the initial reference trajectory based on the feature constraint information includes:

acquiring the current running speed of the vehicle and a preset speed weight;

determining a target vehicle speed of the vehicle after a preset time;

and calculating the vehicle speed cost of the initial reference track based on the current running vehicle speed of the vehicle, the target vehicle speed and a preset vehicle speed weight.

8. The method of claim 7, wherein the determining the target vehicle speed of the vehicle after a preset period of time comprises:

acquiring the following distance between the vehicle and a guiding vehicle, the lane speed limit of the current lane of the vehicle and the state of the guiding vehicle;

and determining the target vehicle speed after the preset time length of the vehicle based on the following distance, the lane speed limit and the guiding vehicle state.

9. The method according to any one of claims 1 to 5, wherein the travel characteristic includes a lane boundary, the basic cost includes a boundary cost, and the determining feature constraint information of the travel characteristic of the vehicle in the initial reference trajectory and calculating the basic cost of the initial reference trajectory based on the feature constraint information includes:

determining a lane left boundary and a lane right boundary of the vehicle according to the current position information of the vehicle;

determining boundary constraint information of the vehicle in the initial reference trajectory based on the lane left boundary and the lane right boundary;

calculating a boundary cost of the initial reference trajectory based on the boundary constraint information.

10. The method according to any one of claims 1 to 5, wherein the running characteristic includes a vehicle acceleration, the basic cost includes an acceleration cost, the determining characteristic constraint information of the running characteristic of the vehicle in the initial reference trajectory, and calculating the basic cost of the initial reference trajectory based on the characteristic constraint information includes:

determining a maximum acceleration and a minimum acceleration of a vehicle acceleration of the vehicle in the initial reference trajectory;

and calculating the acceleration cost of the initial reference track based on the minimum acceleration and the maximum acceleration.

11. The method according to any one of claims 1 to 5, wherein the running characteristic includes a vehicle yaw rate, the basic cost includes an angular velocity cost, and the determining feature constraint information of the running characteristic of the vehicle in the initial reference trajectory and calculating the basic cost of the initial reference trajectory based on the feature constraint information includes:

determining a minimum yaw rate and a maximum yaw rate of a vehicle yaw rate of the vehicle in the initial reference trajectory;

based on the minimum yaw-rate and the maximum yaw-rate, an angular-rate cost of the initial reference trajectory is calculated.

12. The method according to any one of claims 1 to 5, wherein the preset duration includes at least one target time, and the updating the initial reference trajectory by using the global cost to obtain an updated reference trajectory includes:

if the global cost does not meet the convergence condition, reversely calculating the optimal control rate of each target moment in the initial reference track from the last target moment of a preset duration;

determining target track points according to the optimal control rate of each target moment;

and updating the initial reference track based on the target track point until the global cost meets a convergence condition to obtain an updated reference track.

13. A vehicle trajectory optimization device, comprising:

14. A computer readable storage medium storing a plurality of instructions adapted to be loaded by a processor to perform the steps of the vehicle trajectory optimization method of any one of claims 1 to 12.

15. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the steps of the method according to any of claims 1 to 12 are implemented when the program is executed by the processor.