CN114894206A

CN114894206A - Path planning method and device, vehicle and storage medium

Info

Publication number: CN114894206A
Application number: CN202210273527.6A
Authority: CN
Inventors: 潘青贵; 徐成; 张放; 李晓飞; 霍舒豪; 王肖; 张德兆
Original assignee: Beijing Idriverplus Technologies Co Ltd
Current assignee: Beijing Idriverplus Technologies Co Ltd
Priority date: 2022-03-18
Filing date: 2022-03-18
Publication date: 2022-08-12

Abstract

The invention discloses a path planning method and a device, wherein the method comprises the steps of obtaining a high-precision map and an obstacle SL risk field, wherein the obstacle SL risk field is constructed in advance according to multi-frame sensing information of an obstacle; determining a constraint boundary of the selected path planning model according to a high-precision map and an obstacle SL risk field, wherein the obstacle SL risk field is constructed in advance according to multi-frame sensing information of the obstacle; determining a target optimization function of the path planning model based on the cost function; and solving the path planning model according to the determined constraint boundary and the target optimization function to obtain a local path planning result. The scheme of the invention can avoid the defect of unstable local path planning result caused by low accuracy of sensing the position of the obstacle predicted in real time and the like, effectively reduces the influence of the fluctuation of the prediction result on the running safety of the vehicle, and improves the fault-tolerant capability of the system and the comfort of automatic driving.

Description

Path planning method and device, vehicle and storage medium

Technical Field

The present invention relates to the field of automatic driving technologies, and in particular, to a path planning method, a path planning apparatus, a vehicle, and a storage medium.

Background

Artificial intelligence is a theory, method, technique and application system that uses a digital computer or a machine controlled by a digital computer to simulate, extend and expand human intelligence, perceive the environment, acquire knowledge and use the knowledge to obtain the best results. Automatic driving is a mainstream application in the field of artificial intelligence, and the automatic driving technology depends on the cooperative cooperation of computer vision, radar, a monitoring device, a global positioning system and the like, so that a motor vehicle can realize automatic driving without the active operation of human beings.

In the technical field of automatic driving, an automatic driving vehicle encounters a static obstacle stopped on a road or on a roadside in the driving process, and obstacle avoidance path planning is required. At present, an artificial potential field method is generally adopted for obstacle avoidance path planning, and the basic idea of an artificial potential field algorithm is to assume that a target point can generate attraction force on an automatically-driven vehicle and an obstacle can generate repulsion force on the automatically-driven vehicle, so that the automatically-driven vehicle can move along a potential valley between potential peaks. Based on the algorithm thought, the specific implementation scheme of the artificial potential field method is generally as follows: firstly, building a potential field comprising an obstacle potential field and a target point potential field, and then obtaining obstacle repulsive force and target point attractive force borne by a vehicle in the potential field by solving a negative gradient of the potential field; then, superposing the repulsive force of all the obstacles and the attractive force of the target point to obtain the stress condition of the vehicle at any position in the potential field; and finally, continuously and iteratively updating the position according to the resultant force condition, thereby obtaining a complete path from the starting point to the end point.

However, the artificial potential field method only considers the obstacle position information of a single frame, and therefore is easily affected by the fluctuation of the obstacle position given by the front-end sensing module, which further causes instability of a planned local path and even a whole trajectory plan, and further affects comfort of an automatic driving process.

Disclosure of Invention

The embodiment of the invention provides a path planning scheme, which aims to solve the problems that when an artificial potential field method is adopted for path planning in the prior art, potential field building and path planning are carried out on the basis of real-time single-frame obstacle position information provided by perception, high dependence on perception recognition precision and stability is caused, a planned local path is easy to be unstable, and the comfort in a driving process is reduced.

In a first aspect, an embodiment of the present invention provides a path planning method applied to an autonomous vehicle, where the method includes

Acquiring a high-precision map and an obstacle SL risk field, wherein the obstacle SL risk field is constructed in advance according to multi-frame sensing information of an obstacle;

determining a constraint boundary of the selected path planning model according to the high-precision map and the obstacle SL risk field;

determining a target optimization function of the path planning model based on a preset cost function;

and solving the path planning model according to the constraint boundary and the target optimization function to obtain a local path planning result.

In a second aspect, an embodiment of the present invention provides a path planning apparatus applied to an autonomous vehicle, where the apparatus includes

The system comprises an acquisition module, a processing module and a processing module, wherein the acquisition module is used for acquiring a high-precision map and an obstacle SL risk field, and the obstacle SL risk field is constructed in advance according to multi-frame sensing information of an obstacle;

the constraint condition determining module is used for determining a constraint boundary of the selected path planning model according to the high-precision map and the obstacle SL risk field;

the optimization target determining module is used for determining a target optimization function of the path planning model based on a preset cost function;

and the result generation module is used for solving the path planning model according to the determined constraint boundary and the target optimization function to obtain a local path planning result.

In a third aspect, an embodiment of the present invention provides another path planning apparatus, including:

a memory for storing executable instructions; and

a processor for executing executable instructions stored in a memory, which when executed by the processor implement the steps of the above-described method.

In a fourth aspect, an embodiment of the present invention provides a vehicle, including:

the planner is used for planning the path according to the method to obtain a local path planning result; and

and the controller is used for controlling the vehicle according to the local path planning result determined by the planner.

In a fifth aspect, the invention provides a storage medium having stored thereon a computer program which, when executed by a processor, performs the steps of the above method.

In a sixth aspect, the invention provides a computer program product, characterized in that the computer program product comprises a computer program stored on a non-volatile computer-readable storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform the above-mentioned method.

The embodiment of the invention has the beneficial effects that: the method provided by the embodiment of the invention utilizes the pre-constructed SL risk field for path planning to form according to the multi-frame sensing information of the obstacle, so that the precision requirement for the real-time obstacle position prediction of the sensing module is reduced in the constructed SL risk field, the uncertainty of the obstacle position is fuzzified based on the determined constraint condition, and the embodiment of the invention simultaneously utilizes the high-precision map and the obstacle SL risk field formed by the multi-frame sensing information of the obstacle to determine the constraint condition, so that when the single-frame real-time obstacle position of sensing prediction is in error or change, the constraint condition has better inclusion on the obstacle position fluctuation, and cannot be excessively influenced by the single-frame real-time obstacle position error, and the fault tolerance of the path planning model and the local path planning result determined based on the constraint condition can be greatly improved, and the comfort of automatic driving based on the local path planning result is higher.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present invention, 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 some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.

FIG. 1 is a flow chart of a path planning method according to an embodiment of the present invention;

FIG. 2 schematically illustrates a detailed process flow diagram for determining constraint boundaries;

FIG. 3 is a schematic diagram of an environment modeling method for an obstacle risk field according to an embodiment of the present invention;

FIGS. 4A-4C are schematic diagrams of SL coordinate system updates provided by embodiments of the invention;

FIGS. 5A-5C are schematic diagrams of ST coordinate system updates provided by embodiments of the present invention;

FIG. 6 is a schematic diagram of a method for determining a current occupied area of a static obstacle in a current-frame obstacle raster map according to an embodiment of the present invention;

FIG. 7 is a schematic diagram of a method for determining a current occupancy area of a dynamic obstacle in a current frame obstacle grid map according to an embodiment of the present invention;

FIG. 8 is a schematic diagram of a current frame ST risk field determination method for a dynamic obstacle according to an embodiment of the present invention;

FIG. 9 is a schematic diagram of an implementation of step S132 in FIG. 8;

FIG. 10 schematically shows a flowchart of detailed processing of step S31 in FIG. 3 according to an embodiment of the present invention;

fig. 11 schematically shows an effect display diagram of an SL probability grid map including only probability grid point sets obtained after deleting grid points with an occupancy probability smaller than a threshold probability in an obstacle SL risk field;

FIG. 12 schematically shows a flowchart detailing the processing of step S43 in FIG. 10 according to an embodiment of the invention;

FIG. 13 schematically shows a flowchart describing the detailed processing of step S51 in FIG. 12 according to an embodiment of the present invention;

FIG. 14 is a schematic block diagram of a path planning apparatus according to an embodiment of the present invention;

FIG. 15 is a schematic block diagram of a path planning apparatus according to an embodiment of the present invention;

FIG. 16 is a functional block diagram of an autonomous vehicle in accordance with another embodiment of the invention;

fig. 17 is a schematic structural diagram of a path planning apparatus according to an embodiment of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. 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 invention.

It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict.

The invention may be described in the general context of computer-executable instructions, such as program modules, being executed by a computer. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform particular tasks or implement particular abstract data types. The invention may also be practiced in distributed computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed computing environment, program modules may be located in both local and remote computer storage media including memory storage devices.

As used in this disclosure, "module," "device," "system," and the like are intended to refer to a computer-related entity, either hardware, a combination of hardware and software, or software in execution. In particular, for example, an element may be, but is not limited to being, a process running on a processor, an object, an executable, a thread of execution, a program, and/or a computer. Also, an application or script running on a server, or a server, may be an element. One or more elements may be in a process and/or thread of execution and an element may be localized on one computer and/or distributed between two or more computers and may be operated by various computer-readable media. The elements may also communicate by way of local and/or remote processes based on a signal having one or more data packets, e.g., from a data packet interacting with another element in a local system, distributed system, and/or across a network in the internet with other systems by way of the signal.

Finally, it should also be noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.

The path planning method in the embodiment of the invention can be applied to a path planning device, so that a user or automatic driving equipment can obtain a local path planning result by using the path planning device, and the automatic driving equipment is controlled to execute corresponding driving actions according to the local path planning result. Such path planning devices include, for example, but are not limited to, planners on autonomous vehicles, smart tablets, personal PCs, computers, cloud servers, and the like. In particular, the path planning method in the embodiment of the present invention may also be directly applied to an automatic driving device such as an automatic driving vehicle, which is not limited in the present invention.

Fig. 1 schematically shows a path planning method according to an embodiment of the present invention, where an execution subject of the method may be a planner or a controller on an autonomous vehicle, or may also be a processor of a path planning device such as a smart tablet, a personal PC, a computer, a cloud server, or may also be a processor of an autonomous device such as an unmanned cleaning vehicle, an unmanned sweeping vehicle, a sweeping robot, an autonomous vehicle, and the like, and embodiments of the present invention are not limited thereto. The embodiment of the invention is preferably explained in detail by taking the execution subject as a planner on an automatic driving vehicle as an example. As shown in fig. 1, the method of the embodiment of the present invention includes:

step S100: obtaining a high-precision map and an obstacle SL risk field, wherein the obstacle SL risk field is constructed and formed in advance according to multi-frame perception information of obstacles

Step S200: determining a constraint boundary of the selected path planning model according to the acquired high-precision map and the obstacle SL risk field;

step S300: determining a target optimization function of the path planning model based on a preset cost function;

step S400: and solving the path planning model according to the determined constraint boundary and the target optimization function to obtain a local path planning result.

In step S100, a pre-constructed risk field of the obstacle SL may be acquired through a front module or a memory. The SL risk field of the obstacle is constructed in advance according to multi-frame perception information of the obstacle, and may be specifically implemented as an SL probability grid map describing a relationship between grid points and occupation probabilities of the obstacle at the grid points, and may be identified by (s, l, P (s, l)), where (s, l) represents grid points in the SL risk field, and P (s, l) represents occupation probabilities of the grid points (s, l) occupied by the obstacle in the SL risk field. Therefore, the obstacle SL risk field of the embodiment of the invention can effectively describe the relation between each grid point (s, l) of the static obstacle in the obstacle SL calculation result area and the occupation probability P. And by introducing the concept of occupation probability, the obstacle SL risk field can provide accurate position information of grid points occupied by the obstacle and also can provide the probability that each occupied grid point is occupied by the obstacle, the uncertainty fuzzification of the obstacle is realized, a single-frame obstacle SL image is projected without depending on the accurate prediction position of the obstacle, the dependence of path planning on the input accuracy of a perception identification module is liberated, the stability and the comfort of the path planning are improved, and the problems of unreasonable planning result and the like caused by the jitter of the obstacle position prediction can be effectively avoided on the basis of the planned local path planning result.

Exemplarily, the obstacle SL risk field according to the embodiment of the present invention may be implemented as an obstacle SL probability grid map established based on a wavelet coordinate system, that is, implemented as a grid method representation based on the wavelet coordinate system (also called a road coordinate system). Therefore, the risk field of the obstacle SL in the embodiment of the present invention is specifically characterized by using a grid to represent the spatial position of the obstacle in the grid map, i.e., the position in the structured road. In particular, the obstacle risk field in embodiments of the present invention may be characterized as a relation of the spatial position of each grid point in the grid map and its corresponding probability of occupation. A detailed description of a specific implementation of constructing the risk field of the obstacle SL will be given below.

Fig. 3 is a schematic diagram of an obstacle risk field environment modeling method according to an embodiment of the present invention, as shown in fig. 3, the method including:

s11, establishing a current frame obstacle grid map in a frenet coordinate system by taking the current position of the own vehicle as an origin;

in this embodiment, the origin of the spoke coordinate system is set as the current position of the own vehicle, i.e. the projection point of the own vehicle on the center line of the structured road.

In some embodiments, the obstacles may include static obstacles and dynamic obstacles, the type of the obstacle is included in the sensing data, the sensing data is output by a sensing module upstream in a known manner, and a current frame obstacle raster map is established in a frenet coordinate system by taking the current position of the own vehicle as an origin, including:

establishing a current frame SL raster image corresponding to the static obstacle in a freset coordinate system by taking the current position of the own vehicle as an origin; and the number of the first and second groups,

and establishing a current frame ST raster image corresponding to the dynamic barrier in the freset coordinate system by taking the current position of the own vehicle as an origin.

Specifically, a static obstacle is described by using S, L parameters to establish an SL grid map by using the center line of the structured road as a reference line (i.e., the reference line of the own vehicle) in a freeboard coordinate system, where S is the longitudinal distance of the static obstacle projected from the reference line, and L is the transverse distance of the static obstacle projected from the reference line, as shown in fig. 4A, 1 represents the own vehicle, 2 represents the projection point of the static obstacle on the reference line (the projection point is at the longitudinal distance S from the own vehicle), 3 represents the static obstacle, and 4 represents the reference line in the SL coordinate system. For a dynamic obstacle, an ST grid graph may be established, where T is an obstacle time based on the current time, and S is a projected longitudinal distance from a reference line at time T, as shown in fig. 5A, 1 represents a host vehicle, 2 represents a dynamic obstacle, 3 represents a collision point between the dynamic obstacle and the host vehicle, 4 represents a reference line, and 5 represents a predicted trajectory of the dynamic obstacle in an ST coordinate system. This approach may combine perceived obstacles with high-precision maps.

S12, determining the current frame occupied area of the obstacle in the current frame obstacle raster image according to the current frame perception data of the obstacle;

wherein the perception data is output by a perception module known upstream. The sensing module needs to acquire a large amount of environmental information including the state of the vehicle, traffic flow information, road conditions, traffic signs, etc. through various sensors, which mainly include: laser Radar (Lidar), Camera (Camera), Millimeter Wave Radar (Millimeter Wave Radar), and the like.

Taking a static obstacle as an example, fig. 4B shows that the static obstacle determines the occupied area of the obstacle in the previous frame in the obstacle raster image of the previous frame from the previous frame of sensing data; fig. 4C illustrates a current frame occupied area of an obstacle determined by current frame perceptual data of a static obstacle in a current frame obstacle raster map.

And S13, calculating and updating the current frame occupation probability of each barrier in the current frame occupation area in the current frame barrier grid image to obtain a current frame barrier risk field.

Illustratively, the risk fields for static and dynamic obstacles are denoted as SL risk fields and ST risk fields, respectively. The SL risk field mainly describes the relationship between each occupancy grid point (s, l) of a static obstacle and the occupancy probability P, denoted P (s, l). The ST risk field mainly describes the relationship between each occupancy grid point (s, t) of a dynamic obstacle and the occupancy probability P, denoted P (s, t). The static obstacle risk field (i.e., SL risk field) may be denoted as s, l, P (s, l) }, and the dynamic obstacle risk field (i.e., ST risk field) may be denoted as s, t, P (s, t) }.

The method and the device for establishing the obstacle risk field of the current frame are characterized in that the current frame obstacle grid image is established based on a freset coordinate system, after the occupied area of the obstacle in the current frame obstacle grid image is determined, the occupied probability of each grid in the occupied area is calculated, namely the obstacle risk field established by the technical scheme is rasterized and is refined until each grid has the corresponding occupied probability, so that the obstacle risk field is more precise and accurate, and the effectiveness of the obstacle risk field is enhanced.

Before setting forth the invention in more detail, it may be helpful to provide a definition of certain terms used herein to understand the invention.

In the grid map, the occupation area of the static obstacle in the SL grid map contains a plurality of grid points, the upper case SL is defined to represent the collection of grid points contained in the occupation area of a single static obstacle, and the lower front corner mark represents the static stateThe footprint of an obstacle Id, e.g., a static obstacle with Id 5, in the SL grid diagram may be represented as: ( ₅ S, ₅ L); defining a lower case SL to represent a certain grid point in SL, with the upper front corner indicating the grid point number, e.g. the 0 th occupying grid point in the occupied area of a static obstacle with Id 5 may be represented by (c) < 2 > ⁰ ₅ s, ⁰ ₅ l). The occupation area of the dynamic obstacle in the ST grid diagram contains a plurality of grid points, the capital ST is defined to represent the collection of grid points contained in the occupation area of a single dynamic obstacle, the top and bottom corner marks represent the Id of the dynamic obstacle, and the occupation area of the dynamic obstacle in the ST grid diagram, for example, with Id 5, can be represented by (a) ₅ S, ₅ T); defining a lower case ST to represent a certain grid point in ST, and the grid point number is represented by the upper-leading corner mark, for example, the 0 th occupied grid point in the occupied area of the dynamic obstacle with Id of 5 may be represented as: ( ⁰ ₅ s, ⁰ ₅ t)。

Because SL and ST are described based on a freset coordinate system, and the origin points of SL and ST are projection points of the self-vehicle on a reference center line of a structured road, historical risk field longitudinal coordinate data need to be continuously updated according to the motion of the self-vehicle, a lower corner mark m represents the time for establishing a freset coordinate system of a current frame, an upper corner mark n represents the time for establishing the coordinate system of the current frame of data, and the data refer to corresponding coordinate values of s and t of a dynamic obstacle obtained by calculation according to the sensing data of the current frame and the center line of the self-vehicle lane. When superscripts are omitted, it means that the two coordinate systems are time-uniform, e.g. s ₂ ¹ Represents t ₁ Data at t ₂ Projection under the frame of the now coordinate system established at the moment, s ₂ Represents t ₂ Data at time t ₂ And (5) projection under a freset coordinate system established at the moment.

According to the above definition, the current frame occupying area of the static obstacle Id 5 can be represented as (under the current frame fresene coordinate system) ₅ S _m,5 L _m ) Wherein the 0 th grid point can be represented as: ( ⁰ ₅ s _m , ⁰ ₅ l _m )。

For static obstacles, the s-max value is denoted as max _ s, the s-min value is denoted as min _ s, the l-max value is denoted as max _ l, and the l-min value is denoted as min _ l.

For a dynamic obstacle, the tmax value is denoted as max _ t, and the tmin value is denoted as min _ t. The s ranges (min _ s, max _ s) for min _ t and max _ t of the same dynamic obstacle are different under the same time coordinate system, and therefore are distinguished by superscript' ", such as { (min _ s, max _ s), min _ t } and { (min _ s, max _ s), max _ t }.

Fig. 6 and 7 schematically illustrate an implementation method for determining a current occupied area of an obstacle in a current frame obstacle grid map when the obstacle is a static obstacle and a dynamic obstacle, respectively.

As shown in fig. 6, the implementation method for determining the current occupied area of the static obstacle in the current-frame obstacle raster image includes:

s1211: determining a coverage area of the static obstacle according to a position point in current frame sensing data of the static obstacle and the size of the obstacle;

s1212: and projecting the coverage area to a current frame SL grid image to obtain a current frame occupied area of the static obstacle in the current frame SL grid.

The occupation areas of the static obstacle in the grid map of the current frame SL can be represented as (min _ s, max _ s) and (min _ l, max _ l), marking the occupied obstacle Id of the grids in the grid map of SL, each occupied obstacle Id being theoretically unique, since there is no overlap in the actual positions between the obstacles.

It should be noted that, for a static obstacle, the change in the structured road is not large, and therefore, the history frame information may not be considered. As shown in fig. 4B to 4C, fig. 4B is a previous frame occupation area of a static obstacle in a previous frame SL grid map, and fig. 4C is a current frame occupation area of the static obstacle in a current frame SL grid map.

As shown in fig. 7, the implementation method for determining the current occupied area of the dynamic obstacle in the current-frame obstacle raster image includes:

s1221: determining an ST area where the dynamic barrier conflicts with the self vehicle according to the current frame predicted track of the dynamic barrier and the self vehicle reference line;

the determination of the ST area where a dynamic obstacle collides with the own vehicle from the current frame predicted trajectory of the dynamic obstacle and the own vehicle reference line may be represented as { (min _ s, max _ s), min _ t } and { (min _ s ', max _ s'), max _ t }, and the occupied obstacles Id [ ] are marked in the ST grid map, and each occupied obstacle Id is theoretically not unique and therefore marked as Id [ ] due to possible overlap of the predicted trajectories of the dynamic obstacle.

Wherein the predicted trajectory of the current frame of the dynamic obstacle is output by a known upstream prediction module. Like the perception module, the prediction module is also an upstream module of the environment modeling of the present invention, and the present invention is not limited thereto.

It should be noted that the ST area where the dynamic obstacle collides with the own vehicle may be determined according to a method known in the art, and the present invention is not limited thereto. Illustratively, the time t (i.e., min _ t) and s (i.e., min _ s) of a collision starting point during a collision between the host vehicle and the dynamic obstacle and the time t (i.e., max _ t) and s (i.e., max _ s) of a collision ending point are determined according to a current frame predicted trajectory of the dynamic obstacle, the size of the dynamic obstacle, a host vehicle reference line and the size of the host vehicle, and an ST region where the dynamic obstacle collides with the host vehicle is determined according to (min _ t, min _ s), (max _ t, max _ s), and the ST region is not rasterized. For example, a first virtual frame for representing a dynamic obstacle is generated according to the size of the dynamic obstacle, a second virtual frame for representing the vehicle is generated according to the size of the vehicle, each track point in the predicted track of the dynamic obstacle is taken as the central point of the first virtual frame, each road point on the reference line of the vehicle is taken as the central point of the second virtual frame, the first virtual frame is subjected to simulation motion along the predicted track, the second virtual frame is subjected to simulation motion along the reference line, and the time points when the two start to collide and the time points when the two end to collide and the corresponding s-coordinate values are recorded.

S1222: discretizing the ST region grid into a current frame ST grid image to obtain a first estimated occupied region;

as shown in fig. 5C, a first estimated footprint is shown from discretizing the ST region grid into the current frame ST grid map.

S1223: and projecting the occupied area of the previous frame of the dynamic barrier to the grid image of the current frame ST to obtain a second estimated occupied area.

Illustratively, the occupied area of the previous frame of the dynamic obstacle is displaced according to the time variation and the self-vehicle position variation of the previous frame and the current frame, so as to obtain a second estimated area of the dynamic obstacle in the ST grid map of the current frame.

For example, the coordinates of the second estimated occupation area obtained by projecting the previous frame occupation area into the grid map of the current frame ST may be expressed as:

s _m ＝s _m ^m-1 -△s；

t _m ＝t _m ^m-1 -△t；

here, s _m The ordinate, s, representing the moment corresponding to the current frame _m ^m-1 The time difference is a calculation result of a previous frame, namely, a vertical coordinate of a corresponding moment of the previous frame, Δ s is a moving distance of the current frame and the previous frame along the path direction of the vehicle, and can be obtained by calculating the positioning variation of the vehicle, and Δ t is a time difference of two frames, for example, 0.1 second.

In some embodiments, the corresponding time interval (i.e., the update period) between the current frame and the previous frame may be set as needed, for example, determined according to the hardware frequency of the perception sensor or the like. The invention is not limited in this regard.

S1224: and determining a current frame occupying area of the dynamic barrier in a current frame ST grid image according to the first estimated occupying area and the second estimated occupying area.

Illustratively, the first estimated occupation area and the second estimated occupation area are filtered to match with a current frame occupation area of the dynamic barrier in the current frame ST grid map. The processing may be performed by a common filtering method to obtain the current frame occupying area of the dynamic obstacle in the current frame ST grid map. Filtering methods may include, for example, bayesian filtering, kalman filtering, and the like. The invention is not limited in this regard. As shown in fig. 5B and 5C, fig. 5B is a previous frame occupation area of the dynamic obstacle in the previous frame ST grid map, and fig. 5C is a current frame occupation area of the dynamic obstacle in the current frame ST grid map. The occupied area of the previous frame in fig. 5B is projected into the grid map of the current frame ST in fig. 5C, and the projected area and the occupied area of the current frame in fig. 5C are subjected to filtering processing. For the sake of simplicity, the current frame occupied area of the dynamic barrier in the current frame ST grid map obtained by the filtering process is not shown in the figure.

Alternatively, the first and second predicted footprints may be directly superimposed, i.e., a union of the first and second predicted footprints may be taken.

In some embodiments, a method for implementing a risk field of a current frame SL for a static obstacle includes:

and regarding each static obstacle in each grid in the current frame occupying area in the current frame SL grid image, and taking the obstacle position reliability in the grid current frame perception data as the current frame occupying probability of the grid.

Therefore, for static obstacles, the occupation probability of each grid in the current frame can be determined directly according to the perception data.

In some embodiments, a method for determining a current frame ST risk field for a dynamic obstacle, as shown in fig. 8, includes:

s131, calculating and updating the current frame predicted track probability of each grid in the current frame occupied area of each dynamic obstacle in the current frame ST grid image;

s132, determining the current frame occupation probability of each occupied grid in the current frame ST grid map according to the current frame predicted track probability of each dynamic obstacle in each grid in the current frame ST grid map occupation area, so as to obtain a current frame ST risk field.

Exemplarily, with further reference to fig. 9, step S132 may be embodied as:

for each occupied grid in the grid map of the current frame ST, the following steps are performed:

s1321, determining at least one target dynamic obstacle corresponding to each occupied grid according to the current frame occupied area of each dynamic obstacle in the current frame ST grid;

because the dynamic obstacles all correspond to the predicted trajectories, the predicted trajectories may intersect or overlap with each other, so that some occupied grids in the current frame ST grid map may be occupied by multiple dynamic obstacles at the same time, and therefore, when calculating the estimated occupation probabilities of the current frames occupying grids, the predicted trajectory probabilities of the current frames in the occupied grids of the multiple dynamic obstacles respectively need to be considered. Therefore, it is first necessary to determine which target dynamic obstacles are occupying each occupancy grid. For example, it may be determined whether each dynamic obstacle includes the occupancy grid in the current frame occupancy area in the current frame ST grid map, and if so, the dynamic obstacle is determined to be the target dynamic obstacle occupying the occupancy grid.

S1322, determining the estimated occupation probability of the current frame occupying the occupying grid according to the predicted trajectory probability and the preset maximum occupation probability of the current frame occupying the occupying grid respectively occupied by each target dynamic obstacle;

illustratively, the dynamic obstacle Id is represented by i, the occupancy grid is represented by j, and the probability that a single target dynamic obstacle occupies the current frame predicted trajectory of the occupancy grid j is represented by _i P _m ＝ _i k _m The estimated occupation probability of the current frame of the occupation grid j can be expressed as:

P _m ＝min(max_P，∑ _i k _m ) (4)

wherein _i k _m Is Id [ ]]The predicted trajectory probability of the target dynamic obstacle with the median value i in the occupation grid j is, and max _ p is a preset maximum occupation probability.

S1323, calculating to obtain the current frame occupation probability of the occupation grid according to the estimated occupation probability of the current frame of the occupation grid and the occupation probability of the previous frame of the occupation grid.

Illustratively, the current frame occupation probability of the occupation grid can be calculated by the following bayesian filtering formula.

Wherein L is an intermediate variable, and when t is the physical meaning represented by the barrier st _m ^m-1 <At 0, clear the obstacle P (S, T) risk field.

From this, each risk field occupying a grid point P (s, t) can be calculated and updated. The probability value of P (s, t) is high, the stability of the representing prediction result in the time dimension is high, the repeatability in the space dimension is high, a threshold interval can be defined, and the risk level of the intention of the obstacle and the corresponding prediction track are determined.

Illustratively, bayesian filtering may be replaced with other filtering algorithms, such as kalman filtering, etc.

The obstacle risk field calculated and updated by the method of the embodiment of the invention comprises an SL risk field and an ST risk field, not only provides accurate position information, namely the position of an occupied grid point, but also provides probability values P (s, l) and P (s, t) of each grid point, realizes the establishment of an obstacle risk field environment model, and an automatic driving system can carry out decision planning based on the obstacle risk fields { s, l, P (s, l) } and { s, t, P (s, t) }.

In addition, from the perspective of a single frame, a certain error in the accuracy range of the sensor is allowed, and the original result of 'danger existence/nonexistence' is changed into '0-1' probabilistic risk description, so that the applicability of data is improved. Meanwhile, the result superposition of multi-frame continuity improves the stability of data, avoids the influence of single-frame jumping, and improves the accuracy of barrier risk assessment.

According to the technical scheme, when the occupied area of the dynamic barrier in the ST raster image is determined, the ST area where the dynamic barrier of the current frame collides with the own vehicle and the occupied area of the dynamic barrier in the previous frame in the ST raster image of the previous frame are integrated, and the occupied area of the dynamic barrier in the current frame in the ST raster image of the current frame is determined; and when calculating the current frame occupation probability of each occupation grid in the current frame ST grid, integrating the current frame predicted trajectory probability of a plurality of target dynamic obstacles occupying the occupation grid in the occupation grid and the previous frame occupation probability of the occupation grid, and determining the current frame occupation probability of the occupation grid. The technical scheme of the invention can effectively combine the time dimension and the space dimension of the dynamic barrier at the same time, improve the stability and the accuracy of the dynamic barrier risk field and provide accurate environmental model support for more reasonable decision planning.

It should be noted that, in step S100, a high-precision map is also obtained through a front module, such as a real-time high-precision map received from a sensing module or an identification module or a navigation module connected to a planner on the autonomous vehicle.

As a preferred implementation example, the step S200 of determining the constraint boundary of the pre-selected path planning model according to the obstacle SL risk field and the acquired high-precision map is implemented by determining a lane boundary as a first constraint boundary by using the acquired high-precision map and the obstacle SL risk field; and then determining a second constraint boundary according to the determined first constraint boundary and the risk field of the obstacle SL, and constraining the objective function by using the determined first constraint boundary and the determined second constraint boundary as constraint conditions of the path planning model. Fig. 2 schematically shows a specific implementation method for determining the constraint boundary of the pre-selected path planning model in step S200, and as shown in fig. 2, the method may be implemented as a method including:

step S30: determining a first constraint boundary of a pre-selected path planning model according to the acquired high-precision map and the obstacle SL risk field;

step S31: and determining a second constraint boundary of the path planning model according to the first constraint boundary and the risk field of the obstacle SL.

In step S30, the high-precision map may be obtainedThe lane boundary is identified and obtained, and the identification and obtaining method thereof may specifically refer to the prior art, which is not described herein again. After the lane boundary is obtained, the lane boundary is converted into an SL constraint set under the SL risk field according to the constructed obstacle SL risk field to serve as a first constraint boundary. For example, the set of S-values of the first constraint boundary may be first determined according to the coordinate values on the S-axis of the risk field of the obstacle SL and the preset directional resolution on the S-axis, e.g. from the origin S ₀ Starting at 0, with a predetermined directional resolution δ s ═ s _n -s _n-1 Step size in s direction as the first constraint boundary (if δ s ═ s can be set _n -s _n-1 The value range of (1) is 0.1-0.5 m), and the longitudinal distance of the lane boundary is projected to be an S coordinate set on an S axis; then, according to the obtained high-precision map, determining the maximum value L _ max and the minimum value L _ min of coordinate values on the L axis corresponding to each s value in the s value set of the first constraint boundary, and according to each s value and the transverse offset distance of the lane boundary determined by identification at each s value, determining the L value range corresponding to the corresponding s value, thereby forming the L value set L of the first constraint boundary _{hdmap_constraint} ＝{l ₀ ,l ₁ ,...,l _n In which l _i (i ═ 0,1, 2.., n) includes s _i A set of two elements corresponding to a maximum value l _ max and a minimum value l _ min.

In step S31, a constraint adjustment may be performed on the first constraint boundary according to a relationship between the grid points described in the risk field of the obstacle SL and the occupation probability of the obstacle at each grid point, so as to determine a second constraint boundary based on the risk field of the obstacle SL. Fig. 10 schematically shows a specific implementation example of step S31, and as shown in fig. 10, determining the second constraint boundary of the path planning model according to the risk field of the obstacle SL and the first constraint boundary may specifically be implemented as follows:

step S40: determining a probability threshold value of grid points which cannot be crossed in the SL risk field;

step S41: screening a probability grid point set from the obstacle SL risk field according to the occupation probability of each grid point and the determined probability threshold, wherein the probability grid point set is a set of grid points which cannot be passed through;

step S42: projecting each grid point in the probability grid point set to a reference line SL coordinate system as an independent obstacle, and determining the projection coordinate of each grid point under the reference line SL coordinate system;

step S43: and determining a second constraint boundary of the path planning model according to the determined projection coordinate of each grid point and the first constraint boundary.

The pre-constructed obstacle SL probability grid graph reflects the probability of occupying each grid point position on the self-vehicle planned path along with the change of time and self-vehicle motion, and is formed according to multi-frame sensing data, the higher the probability value of the occupation probability P (s, t), the higher the stability of the prediction result in the time dimension is represented, and the higher the repeatability in the space dimension is represented, so that when the occupation probability is greater than a certain threshold value, the grid point is considered to be no-crossing, namely, the path planning needs to avoid all grids which are not allowed to cross and simultaneously to be far away from the grids with the occupation probability as far as possible. Therefore, a threshold interval may be defined according to the probability value of the occupation probability, and the occupation probability prohibited from crossing is determined as a screening condition for performing constraint boundary optimization according to the embodiment of the present invention, that is, the probability threshold p in step S40 _max . In the embodiment of the invention, the determined probability threshold value p is used _max As a determination criterion for determining the grid points which are prohibited from crossing, the grid points which have an occupancy probability less than the probability threshold are regarded as traversable grid points, and the grid points which have an occupancy probability reaching the probability threshold are regarded as strictly prohibited from crossing.

In step S41, all grid points in the constructed risk field of the obstacle SL will be traversed to filter out the set of probability grid points. As a preferred implementation example, in order to facilitate the grid point screening, the embodiment of the present invention may set the range of the directional resolution δ s and the range of the value l according to the length and the width of the grid point in the constructed risk field of the obstacle SL, and exemplarily, the directional resolution δ s may be set as the length of one grid point, and each s value may be set as the length of one grid points _i The difference δ l ═ l _ max-l _ min between the corresponding maximum value l _ max and minimum value l _ min is set to the width of one grid point. Taking the directional resolution δ S preset in the risk field of the obstacle SL as the length of one grid point and the width of each grid point δ l as an example, the probability grid point set can be screened by traversing along the S-axis direction in the risk field of the obstacle SL, for example, taking S as an example ₀ And traversing the grid points in the obstacle SL risk field by taking the directional resolution deltas of the obstacle SL risk field as a step length, and deleting the grid points with the occupation probability smaller than the threshold probability to obtain a set of obstacle grid points which are all forbidden to traverse, wherein the set is taken as a starting point of traversal and is called as a set of probability grid points in the embodiment of the invention. Therefore, according to the screened probability grid point set, the grid points are avoided as planning targets, and path planning is carried out. Fig. 11 schematically shows the display effect of the SL probability grid map only including the probability grid point set after deleting the grid points with the occupation probability smaller than the threshold probability in the SL risk field of the obstacle, as shown in fig. 11, each square in the map represents one grid point (referred to as a probability grid point in the embodiment of the present invention) which is forbidden to pass through, wherein each probability grid point in the map preferably has a length δ s and a width δ l.

In step S42, determining the projection coordinate of each grid point in the probability grid point set may be determined according to the length and width values of the probability grid point and combining the sl value of each grid point in the obstacle risk field, for example, the projection coordinate of each probability grid point may be determined as P _sl ＝{(s_min,s_max),(l_min,l_max)}＝{(s _i ，s _i +δs)，(l _i ，l _i + δ l) }, where, s _i ,l _i Coordinates of grid points in the obstacle risk field. In step S42, the reference line SL coordinate system constructed may refer to the coordinate system construction manner of the obstacle SL risk field, that is, the SL coordinate system established with the projected point of the host vehicle on the center line of the roadway as the origin, the tangential direction of the reference line as the S axis, and the normal direction of the reference line as the L axis.

In step S43, the first constraint boundary may be optimized by evaluating the set of l valuesAnd performing chemical adjustment to take the l value set after optimization adjustment as the l value set of the second constraint boundary, and directly taking the s value set of the first constraint boundary as the corresponding s value set of the second constraint boundary, thereby obtaining the second constraint boundary. Illustratively, the determined second constraint boundary includes a set of L values L _{slmap_constraint} ＝{L ₀ ,L ₁ ,...,L _n In which L is _i (i ═ 0,1, 2.., n) includes s _i And the corresponding set of the maximum value l _ max and the minimum value l _ min is obtained by optimizing and adjusting the set of l values of the first constraint boundary by using the probability grid point set.

Taking the pre-selected path planning model as a polynomial function as an example, the following describes an exemplary specific optimization process of step S43.

As a preferred implementation, a fifth order polynomial may be selected for path planning modeling. Illustratively, the path planning model is determined by the function l ═ f(s) ═ a ₅ l ⁵ +a ₄ l ⁴ +a ₃ l ³ +a ₂ l ² +a ₁ l+a ₀ Where s is a tangential direction along a reference line (i.e., a center line of a road), l is a normal direction along the reference line, and a ₀ -a ₅ Are the variables to be solved. Accordingly, the constraint boundaries determined for the polynomial path planning model are exemplarily set to include a first constraint boundary on the lane boundary and a second constraint boundary in the risk field of the obstacle SL.

Taking the length of each probability grid point as δ s and the width as δ l, and the coordinate of each probability grid point as an independent obstacle projected to the reference line SL coordinate system as P _sl ＝{(s_min,s_max),(l_min,l_max)}＝{(s _i ，s _i +δs)，(l _i ，l _i + δ L) }, a set L of values L of the determined first constraint boundary _{hdmap_constraint} ＝{l ₀ ,l ₁ ,...,l _n In which l _i (i ═ 0,1, 2.., n) is a set containing two elements l _ max, l _ min, where l _ max, l _ min are s ═ s, respectively _i Corresponding maximum and minimum values of δ s ═ s _n -s _n-1 Arranged in a risk field with obstacles SLAs an example, the S-directional resolution δ S is kept consistent, fig. 12 schematically shows a specific processing procedure of step S43 in this example, and as shown in fig. 12, the specific processing procedure of determining the second constraint boundary of the path planning model according to the determined projection coordinates of each grid point and the first constraint boundary is preferably implemented as follows:

step S50: initializing a centerline of a first constraint boundary;

step S51: traversing the first constraint boundary by taking the directional resolution on the S axis of the SL risk field as a search step, and determining the corresponding L in the L value set of the second constraint boundary according to the matching relation between the SL value in the first constraint boundary and the grid points in the probability grid point set in the traversing process _i And updating the centerline of the first constraint boundary.

For example, in step S50, the center line of the first constraint boundary may be set to l _ center _ line ═ f (S), and the initial value may be set to l _ center _ line ═ 0.

In step S51, the following traversal process may be specifically performed as shown in fig. 13 to implement the determination of the set of l values of the second constraint boundary:

starting from the initial point of the first constraint boundary, s-0, i-0, under all sets of s values, i.e. i<Under the condition of n, traversing the first constraint boundary by taking the traversed search step as deltas, and in each traversal process, carrying out traversal on the current s value s in the s value set of the first constraint boundary _i Judging s according to the determined projection coordinate of each grid point _i If the probability exists in the probability grid point set, if so, judging s _i If the probability grid point exists in the probability grid point set, the corresponding l value l in the l value set of the first constraint boundary is determined according to the projection coordinate of the corresponding grid point _i Determining L in a set of L values for a second constraint boundary _i (ii) a If not, directly setting the corresponding l value l in the l value set of the first constraint boundary _i Determined as L in the second set of constraint boundaries _i 。

As a preferred implementation, s is judged according to the determined projection coordinates of each grid point _i Whether or not there exists a set of probability grid pointsIn (3), can be determined by judging the current s _i Projection coordinate P of certain grid point in probability grid point set _sl Is realized within the range of the s value of { (s _ min, s _ max), (l _ min, l _ max) }, i.e. (s _ min, s _ max), i.e. if s is currently present _i If the first constraint boundary is within the range of (s _ min, s _ max) of the projection coordinate of a certain grid point in the probability grid point set, the probability grid point exists in the current s direction of the first constraint boundary, namely the grid point which cannot be crossed exists, and obstacle path avoidance is needed, otherwise, avoidance is not needed. Thus, when it is determined that there is a probability grid point in the current s direction of the current first boundary constraint, it may be determined from the projection coordinates of the corresponding grid point and the corresponding l value/in the set of l values of the first constraint boundary _i Determining L in a set of L values for a second constraint boundary _i Otherwise (i.e. if no probability grid point exists on the current s), the corresponding l value l in the set of l values of the first constraint boundary may be directly put together _i Determined as L in the second set of constraint boundaries _i I.e. set L _i ＝l _i . Therefore, the constraint boundary can be optimally adjusted according to the occupation probability of the obstacle in different grid point positions determined in the obstacle SL risk field, and more stable obstacle avoidance path planning is realized.

Preferably, the respective l-value/in the set of l-values according to the projection coordinates of the respective grid point and the first constraint boundary _i Determining L in a set of L values for a second constraint boundary _i During the process, the condition that each probability grid point deviates from the central line can be further judged, and L in the L value set of the second constraint boundary is determined according to the deviation amplitude of each probability grid point from the central line _i . For example, the determination process may be specifically implemented by determining whether P exists for the probability grid point corresponding to the current s value (i.e. the probability grid point whose current s value is within the s value range of the projection coordinates thereof) _sl .l_min+P _sl .l_max<L _ center _ line 2. if the constraint boundary L exists, the current constraint boundary L is determined to exist _i Is determined as L _i ＝(P _sl .l_max,l _i L _ max); if not, the current constraint boundary L is set _i Is determined as L _i ＝(l _i .l_min,P _sl L _ min). Wherein, P _sl L _ min refers to the projection coordinate P of the currently searched probability grid point _sl Minimum value of l value l _ min, P _sl L _ max refers to the projection coordinate P of the currently searched probability grid point _sl The maximum value l _ max, l of the value l in (1) _i L _ min means l _i Minimum value of l _ min, l _i L _ max means l _i Maximum value l _ max in (1). By further judging the offset amplitude of each probability grid point from the central line, the method can more accurately adapt to the position of the obstacle avoidance target to carry out boundary planning, so as to further improve the stability of path planning and the comfort of automatic driving.

In step S51, L of the second constraint boundary is determined _i May also be based on L in the set of L values of the second constraint boundary _i The centerline of the first constraint boundary is updated. Illustratively, it may be implemented to update the constraint boundary centerline L _ center _ line to L _ center _ line-0.5 (L) _i .l_min+L _i L _ max), wherein L _i L _ min means L _i Minimum value of L _ min, L _i L _ max means L _i Maximum value l _ max in (1).

In other embodiments, before traversal, both the s-value and the L-value set of the second boundary constraint may be initialized to be the same as the first constraint boundary, and in the traversal process, when it is determined that a probability grid point exists in the current s direction, the L-value L corresponding to the current s of the second constraint boundary is determined according to the above implementation scheme _i Updating, and when judging that no probability grid point exists in the current s direction, not updating the L value L corresponding to the current s of the second constraint boundary _i Update is made but determined as l with the first constraint boundary _i The same, i.e. the initialized value.

Therefore, the embodiment of the invention can determine the constraint conditions through the high-precision map and the obstacle SL risk field at the same time, the determined constraint conditions comprise the constraint boundary (namely a first constraint boundary) defined by the lane boundary of the actual map and the constraint boundary (namely a second constraint boundary) defined by the probability grid points which cannot be crossed in the obstacle SL risk field, so that the planned local path planning result can better accord with the planning purpose, and the constraint conditions can limit the constraint boundary by considering the occupation probability of the obstacle without depending on the accurate position of the obstacle, so that the planned local path planning result has better stability, and the automatic driving comfort is higher.

As a preferred implementation example, when optimizing the path planning model, the embodiment of the present invention preferably aims to make the local path planning result as far as possible from the obstacle, and thus in step S300, the selected cost function at least includes a first cost function that makes the local path planning result defined by the path planning model as far as possible from the obstacle.

Preferably, the first cost function is constructed and formed based on projection coordinates of probability grid points in the risk field of the obstacle SL and the obstacle occupation probability of the probability grid points, so that a concept of the obstacle occupation probability is introduced into an optimization target to fuzzify the accurate position of the obstacle, multi-frame sensing data can be comprehensively considered in the planned local path planning result, the influence of single-frame sensing data on the fluctuation of the predicted position of the obstacle is avoided, the stability of the planned local path planning result is improved, the defects of sudden braking, easiness in collision and the like caused by unreasonable prediction results are avoided, and the comfort of automatic driving is improved. Illustratively, the first cost function may be constructed as defined by the following mathematical expression:

Cost_2＝w ₄ *p*∑(f(s)-l _n ) ² ；

wherein l is _n Is the value of the projection coordinate of each probability grid point, p is the obstacle occupation probability of the probability grid points in the probability grid point set, f(s) is the selected path planning model, w ₄ Is the corresponding weight coefficient.

In a more preferred implementation, when the path planning model is optimized, the embodiment of the present invention further adds an optimization target for smoothing the local path planning result as much as possible, so that in step S300, the selected cost function may further include a second cost function for smoothing the local path planning result defined by the path planning model as much as possible. Illustratively, the second cost function may be constructed as defined by the following mathematical expression:

Cost_1＝w ₁ *∑f(s) ² +w ₂ *∑f’(s) ² +w ₃ *Σf”(s) ²

wherein f(s) is the selected path planning model, f'(s) is the first derivative of the selected path planning model, f "(s) is the second derivative of the selected path planning model, w ₁ -w ₃ Respectively, corresponding weight coefficients.

As a preferred implementation, the objective optimization function determined according to the cost function in the embodiment of the present invention is implemented to include both the first cost function and the second cost function, so that the optimization objective can also make the planned path curve as far away from the grid point position with high probability of being occupied by the obstacle as possible on the basis that the local path planning result is as gentle as possible. Illustratively, the objective optimization function may be implemented as defined by the following mathematical expression:

after the constraint boundary and the objective optimization function are determined through the above processing, the constraint boundary and the objective optimization function can be used to solve the pre-selected path planning model, for example, the objective optimization function is optimized based on the first constraint boundary and the second constraint boundary with the minimum cost of the objective optimization function, and then the variable a to be solved in the path planning model, such as a polynomial, can be obtained ₀ -a ₅ And obtaining a path planning model meeting the constraint boundary and the optimization target, and correspondingly obtaining a corresponding local path planning curve as a local path planning result according to the model.

According to the embodiment of the invention, the constraint boundary is determined simultaneously by using the constructed obstacle SL risk field and the high-precision map acquired in real time, and the optimization target far away from the obstacle as far as possible is added into the target optimization function, so that the dependence of the whole path planning scheme on the identification precision of the sensing module is greatly reduced, the planned path result is not influenced by the fluctuation of the predicted position of the obstacle input by the sensing module, the stability and the comfort of the planned path curve are more excellent, the automatic driving vehicle is controlled to run by using the local path planning result, the bad phenomena of sudden braking, failure in effective obstacle avoidance and the like caused by the fluctuation of the sensing result can be effectively avoided, and the comfort of automatic driving is improved.

FIG. 14 schematically shows a path planning apparatus according to an embodiment of the present invention, which includes, as shown in FIG. 14

An obtaining module 63, configured to obtain a high-precision map and the SL risk field, where the SL risk field is pre-constructed and formed according to multi-frame sensing information of an obstacle;

a constraint condition determining module 60, configured to determine a constraint boundary of the selected path planning model according to the high-precision map and the risk field of the obstacle SL;

an optimization objective determination module 61, configured to determine an objective optimization function of the path planning model based on a preset cost function; and

and the result generation module 62 is configured to solve the path planning model according to the determined constraint boundary and the objective optimization function to obtain a local path planning result.

As a preferred embodiment, the constraint boundaries determined for the path planning model include a first constraint boundary and a second constraint boundary, and the constraint condition determining module 60 is specifically implemented to include:

the first condition determining unit is used for determining a first constraint boundary of the selected path planning model according to the high-precision map and the obstacle SL risk field; and

and the second condition determining unit is used for determining a second constraint boundary of the path planning model according to the first constraint boundary and the SL risk field.

As a preferred implementation, the SL risk field is implemented as an SL probability grid map describing the relationship between grid points and the probability of occupation of an obstacle at each grid point, and the second condition determining unit is specifically configured to

Determining a probability threshold value of grid points which cannot be crossed in the SL risk field;

screening a probability grid point set from the obstacle SL risk field according to the occupation probability of each grid point and the determined probability threshold, wherein the probability grid point set is a set of grid points which cannot be passed through;

projecting each grid point in the probability grid point set to a reference line SL coordinate system as an independent obstacle, and determining the projection coordinate of each grid point under the reference line SL coordinate system;

and determining a second constraint boundary of the path planning model according to the determined projection coordinates of each grid point and the first constraint boundary.

In a preferred embodiment, the cost function constructed by the optimization objective determination module 61 comprises at least a first cost function that makes the local path planning result defined by the path planning model as far away from the obstacle as possible.

Preferably, the first cost function is formed based on the projection coordinates and the occupation probability construction of each grid point in the probability grid set.

Illustratively, the first cost function is constructed as defined by the following mathematical expression:

Cost_2＝w ₄ *p*∑(f(s)-l _n ) ² ；

wherein l _n Is the value of the projection coordinate of each probability grid point, p is the obstacle occupation probability of the probability grid points in the probability grid point set, f(s) is the selected path planning model, w ₄ Is the corresponding weight coefficient.

As another preferred embodiment, the cost function constructed by the optimization objective determination module 51 further includes a second cost function that makes the local path planning result defined by the path planning model as smooth as possible. Illustratively, the second cost function may be constructed as defined by the following mathematical expression:

Cost_1＝w ₁ *∑f(s) ² +w ₂ *∑f’(s) ² +w ₃ *Σf”(s) ²

wherein f'(s) is the first derivative of the selected path planning model, f "(s) is the second derivative of the selected path planning model, w ₁ -w ₃ Respectively, corresponding weight coefficients.

As a preferred implementation, the objective optimization function may be implemented as defined by the following mathematical expression:

preferably, the high-precision map obtained by the constraint condition determination module 60 is obtained by a perception module, a recognition module and/or a front module.

It should be noted that, for the specific implementation process of each module and unit of the path planning apparatus in the embodiment of the present invention, reference may be made to the description of the foregoing method portion, and therefore, no further description is given here.

Fig. 15 schematically shows a path planning apparatus according to another embodiment of the present invention, which is implemented to include, as shown in fig. 15:

a memory 70 for storing executable instructions; and

a processor 71 for executing executable instructions stored in a memory, which when executed by the processor implement the steps of the path planning method according to any of the preceding embodiments.

In specific practice, the path planning apparatus may be exemplarily applied to an autonomous device such as an autonomous vehicle, an unmanned cleaner, an unmanned sweeper, and a robot to implement local path planning on the device, so as to improve the comfort of the unmanned vehicle.

Fig. 16 schematically shows a vehicle according to an embodiment of the present invention, which includes, as shown in fig. 16:

a planner 80, configured to perform path planning according to the method of any of the embodiments to obtain a local path planning result; and

and the controller 81 is used for controlling the vehicle according to the local path planning result determined by the planner 80, such as direction control, speed planning and the like.

In other embodiments, the planner and the controller in the vehicle may also be integrated into one control module. Optionally, in practical applications, the vehicle may also be an autonomous vehicle, and the autonomous vehicle may further include a perception identification module and other planning control modules, such as a path planning controller, an underlayer controller, and the like, which is not limited in this embodiment of the present invention.

In some embodiments, the present invention provides a non-transitory computer-readable storage medium, in which one or more programs including executable instructions are stored, where the executable instructions can be read and executed by an electronic device (including but not limited to a computer, a server, or a network device, etc.) to perform the path planning method according to any one of the above embodiments of the present invention.

In some embodiments, the present invention further provides a computer program product comprising a computer program stored on a non-volatile computer-readable storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform the path planning method of any of the above embodiments.

In some embodiments, an embodiment of the present invention further provides an electronic device, which includes: the system comprises at least one processor and a memory which is connected with the at least one processor in a communication mode, wherein the memory stores instructions which can be executed by the at least one processor, and the instructions are executed by the at least one processor so as to enable the at least one processor to execute the path planning method of any one embodiment.

In some embodiments, the present invention further provides a storage medium, on which a computer program is stored, where the computer program is executed by a processor to implement the path planning method according to any one of the above embodiments.

Fig. 17 is a schematic hardware structure diagram of a path planning apparatus according to another embodiment of the present invention, where the path planning apparatus can be implemented by the structure shown in the diagram, and as shown in fig. 17, the path planning apparatus includes:

one or more processors 610 and a memory 620, with one processor 610 being an example in fig. 17.

The path planning apparatus may further include: an input device 630 and an output device 640.

The processor 610, the memory 620, the input device 630, and the output device 640 may be connected by a bus or other means, and are exemplified by being connected by a bus in fig. 17.

The memory 620, which is a non-volatile computer-readable storage medium, may be used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as program instructions/modules corresponding to the path planning method in the embodiments of the present application. The processor 610 executes various functional applications of the server and data processing by running nonvolatile software programs, instructions and modules stored in the memory 620, that is, implements the path planning method of the above-described method embodiment.

The memory 620 may include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the path planning method, and the like. Further, the memory 620 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 non-volatile solid state storage device. In some embodiments, memory 620 optionally includes memory located remotely from processor 610, which may be connected to the electronic device via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.

The input device 630 may receive input numeric or character information and generate signals related to user settings and function control of the image processing apparatus. The output device 640 may include a display device such as a display screen.

The one or more modules are stored in the memory 620 and, when executed by the one or more processors 610, perform a path planning method in any of the method embodiments described above.

The product can execute the method provided by the embodiment of the application, and has the corresponding functional modules and beneficial effects of the execution method. For technical details that are not described in detail in this embodiment, reference may be made to the methods provided in the embodiments of the present application.

The electronic device of the embodiments of the present application exists in various forms, including but not limited to:

(1) mobile communication devices, which are characterized by mobile communication capabilities and are primarily targeted at providing voice and data communications. Such terminals include smart phones (e.g., iphones), multimedia phones, functional phones, and low-end phones, among others.

(2) The ultra-mobile personal computer equipment belongs to the category of personal computers, has calculation and processing functions and generally has the characteristic of mobile internet access. Such terminals include PDA, MID, and UMPC devices, such as ipads.

(3) Portable entertainment devices such devices may display and play multimedia content. Such devices include audio and video players (e.g., ipods), handheld game consoles, electronic books, as well as smart toys and portable car navigation devices.

(4) The server is similar to a general computer architecture, but has higher requirements on processing capability, stability, reliability, safety, expandability, manageability and the like because of the need of providing highly reliable services.

(5) And other electronic devices with data interaction functions.

The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment.

Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a general hardware platform, and certainly can also be implemented by hardware. Based on such understanding, the above technical solutions substantially or contributing to the related art may be embodied in the form of a software product, which may be stored in a computer-readable storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method according to the embodiments or some parts of the embodiments.

Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions in the embodiments of the present application.

Claims

1. A method of path planning, the method comprising:

2. The method according to claim 1, characterized in that the cost function comprises at least a first cost function that makes the local path planning result defined by the path planning model as far away from obstacles as possible.

3. The method of claim 2, wherein the cost function further comprises a second cost function that makes the local path planning result defined by the path planning model as smooth as possible.

4. The method according to any one of claims 1 to 3, wherein the constraint boundaries comprise a first constraint boundary and a second constraint boundary, and the determining the constraint boundaries of the selected path planning model according to the high-precision map and the risk field of the obstacle SL comprises:

determining a first constraint boundary of the selected path planning model according to the high-precision map and the obstacle SL risk field;

and determining a second constraint boundary of the path planning model according to the first constraint boundary and the risk field of the obstacle SL.

5. The method according to claim 4, wherein the obstacle SL risk field is implemented as an SL probability grid map describing a relationship between grid points and occupancy probabilities of obstacles at the grid points, and wherein determining the second constraint boundary of the path planning model from the first constraint boundary and the obstacle SL risk field specifically comprises:

6. The method according to claim 5, wherein the determining a first constraint boundary of a pre-selected path planning model according to the acquired high-precision map and the SL risk field specifically comprises:

determining a set of S values of a first constraint boundary according to coordinate values on an S axis of the obstacle SL risk field and a preset directional resolution on the S axis;

determining the maximum value L _ max and the minimum value L _ min of coordinate values on the L axis corresponding to each s value in the s value set of the first constraint boundary according to the acquired high-precision map to form the L value set L of the first constraint boundary _{hdmap_constraint} ＝{l ₀ ,l ₁ ,...,l _n In which l _i (i ═ 0,1, 2.., n) includes s _i A set of two elements corresponding to a maximum value l _ max and a minimum value l _ min.

7. The method of claim 6, wherein the second constraint boundary comprises a set of L-values L _{slmap_constraint} ＝{L ₀ ,L ₁ ,...,L _n In which L is _i (i-0, 1, 2.., n) is a group containing s _i Determining a second constraint boundary of the path planning model according to the determined projection coordinates of each grid point and the first constraint boundary, wherein the determining the second constraint boundary of the path planning model comprises:

initializing a centerline of a first constraint boundary;

traversing the first constraint boundary by taking the directional resolution on the S axis of the obstacle SL risk field as a search step, and performing the following processing in the traversing process:

for the current s value s in the set of s values of the first constraint boundary _i From each determined grid pointProjection coordinate determination s _i If s is present in the set of probability grid points _i If the probability grid point exists in the probability grid point set, the corresponding l value l in the l value set of the first constraint boundary is determined according to the projection coordinate of the corresponding grid point _i Determining L in a set of L values for a second constraint boundary _i (ii) a If not, directly setting the corresponding l value l in the l value set of the first constraint boundary _i Determined as L in the second set of constraint boundaries _i ；

L in the set of L values according to a second constraint boundary _i The centerline of the current first constraint boundary is updated.

8. A path planner, the device comprising:

the system comprises an acquisition module, a processing module and a display module, wherein the acquisition module is used for acquiring a high-precision map and an obstacle SL risk field, and the obstacle SL risk field is constructed in advance according to multi-frame perception information of an obstacle;

the optimization target determination module is used for determining a target optimization function of the path planning model based on a preset cost function;

9. The path planner according to claim 8, wherein the constraint condition determination module comprises:

and the second condition determining unit is used for determining a second constraint boundary of the path planning model according to the first constraint boundary and the risk field of the obstacle SL.

10. The path planner as claimed in claim 9, wherein the obstacle SL risk field is implemented as an SL probability grid map describing a relationship between grid points and occupancy probabilities of obstacles at the grid points, the second condition determining unit being specifically configured to:

11. A path planning apparatus, comprising:

a memory for storing executable instructions; and

a processor for executing executable instructions stored in a memory, which when executed by the processor implement the steps of the method of any one of claims 1 to 7.

12. A vehicle, characterized in that the vehicle comprises:

a planner for performing a path planning according to the method of any one of claims 1 to 7 to obtain a local path planning result;

13. A storage medium on which a computer program is stored which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 7.

14. A computer program product, characterized in that the computer program product comprises a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform the method of any of claims 1-7.