CN114812585A - Path planning method and device - Google Patents

Abstract

The embodiment of the present invention discloses a path planning method and device. The method includes: obtaining current decision planning related information corresponding to a target object; Planning-related information and preset driving constraints, determine the reachable state set corresponding to the target object; obtain the planned trajectory information corresponding to each reachable state in the reachable state set; based on the planned trajectory information corresponding to each reachable state, each reachable state The level corresponding to the state and the evaluation factor, determine the target state corresponding to the target object from all the reachable states, wherein the level corresponding to the reachable state is the level in the preset hierarchical state machine; control the preset hierarchical state The machine jumps from the current state to the target state, so that the target object travels based on the planning trajectory information corresponding to the target state, so as to improve the rationality of the path planning.

Description

Path planning method and device

technical field

The present invention relates to the technical field of path planning, and in particular, to a path planning method and device.

Background technique

In high-speed and urban road traffic, the path planning module of the autonomous vehicle needs to plan a reasonable trajectory in real time according to the map task and meet the traffic regulations and the driving habits of normal human drivers, and hand it over to the underlying control module for execution. The state of the self-driving vehicle is updated and maintained in the interface, so as to show the intention of the self-driving vehicle and the corresponding driving behavior to the passengers of the self-driving vehicle and the interacting pedestrians and vehicles around it.

At present, in the process of planning the trajectory, the path planning module generally first directly determines the target state based on the jump relationship between the states of the preset state machine and the current state, and then directly determines the target state based on the current environment collected by the upper-layer information collection module. Information, map information and the pose information of the autonomous vehicle, as well as the target state to be jumped to and the preset path planning algorithm, plan a reasonable trajectory.

In the above process, based on the jump relationship between the states of the preset state machine and the current state, the target state is directly determined, and the determined target state may not be the optimal target state, and then the determined trajectory is determined. Probably not the most reasonable trajectory.

SUMMARY OF THE INVENTION

The present invention provides a path planning method and device to improve the rationality of path planning. The specific technical solutions are as follows:

In a first aspect, an embodiment of the present invention provides a path planning method, the method includes:

Obtain the relevant information of the current decision-making plan corresponding to the target object;

Based on the current state of the preset hierarchical state machine, the jump relationship between the states, the relevant information of the current decision plan, and the preset driving limit conditions, a set of reachable states corresponding to the target object is determined, and the preset The hierarchical state machine includes: a jump relationship and a hierarchical relationship between states corresponding to the target object;

obtaining planning trajectory information corresponding to each reachable state in the set of reachable states;

Based on the planning trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state, the target state corresponding to the target object is determined from all reachable states, wherein the level corresponding to the reachable state is its Preset levels in a hierarchical state machine;

The preset hierarchical state machine is controlled to jump from the current state to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

Optionally, the current decision planning related information includes: current surrounding perception information, current pose information, current map information, and prediction information of obstacles of the target object.

Optionally, determining the reachable state corresponding to the target object based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision-making planning related information, and the preset driving limit condition. The steps of the collection include:

Preprocessing the specified information in the current decision planning related information, and judging whether each information in the current decision planning related information is valid;

In the case where it is determined that the obtained current decision planning related information is valid, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, A reachable state set corresponding to the target object is determined.

Optionally, the step of determining the target state corresponding to the target object from all the reachable states based on the planning trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state, includes:

Judging whether there is a reachable state in which the corresponding execution label is a must-execute label in the reachable state set, wherein the jump label is: based on the current surrounding perception information and/or the current surrounding perception information in the current decision planning related information Labels as determined by map information;

If it is determined that there is a reachable state in the set of reachable states in which the corresponding execution label is a mandatory label, based on the current priority corresponding to the top-level state corresponding to each reachable state, determine the reachable state corresponding to the highest current priority. state, as the state to be evaluated; based on the planning trajectory information corresponding to the state to be evaluated and the evaluation factors corresponding to the state to be evaluated, determine the evaluation result of whether the planned trajectory information corresponding to the state to be evaluated is feasible. Status has the highest priority;

If the evaluation result indicates that the planned trajectory information corresponding to the state to be evaluated is feasible, then based on the feasible planned trajectory information, the target state corresponding to the target object is determined from the state to be evaluated;

If the evaluation result indicates that the planning trajectory information corresponding to the state to be evaluated is infeasible, return to execute the current priority corresponding to the top-level state corresponding to each reachable state, and determine the reachable state corresponding to the highest current priority , as the step of the state to be evaluated.

Optionally, if the to-be-evaluated state includes reachable states that share a common parent state with each other and are at the same level; there is a temporal phase transition relationship between the reachable states that share a common parent state and are at the same level;

The step of determining whether the planned trajectory information corresponding to the to-be-evaluated state is feasible based on the planned trajectory information corresponding to the to-be-evaluated state and the evaluation factor corresponding to the to-be-evaluated state includes:

For the to-be-evaluated states that share a common parent state and are at the same level, based on the sequence of the transitional relationships of the time-series stages corresponding to each to-be-evaluated state, determine in turn the currently unevaluated and the first reachable state of the corresponding time-series-stage transition relationship , which is the current state to be evaluated;

Determine the evaluation index value corresponding to the current state to be evaluated based on the planning trajectory information corresponding to the current state to be evaluated and the evaluation factor corresponding to the current state to be evaluated;

If the rating index value corresponding to the current to-be-evaluated state represents: the planned trajectory information corresponding to the current to-be-evaluated state is feasible, determine whether there is an unevaluated state in the to-be-evaluated state;

If it is judged to exist, returning to the state to be evaluated, and determining the unevaluated and the first reachable state of the corresponding sequence stage transition relationship, as the step of the current state to be evaluated;

If it is judged that it does not exist, or it is determined that the planned trajectory information corresponding to the current state to be evaluated is infeasible, an evaluation result of whether the planned trajectory information corresponding to the state to be evaluated is feasible is obtained.

Optionally, the step of determining the target state corresponding to the target object from the to-be-evaluated state based on feasible planning trajectory information includes:

Among the reachable states corresponding to the feasible planning trajectory information, the last reachable state of the corresponding time sequence phase transition relationship is determined as the target state corresponding to the target object.

Optionally, the method further includes:

If it is determined that there is no reachable state in the reachable state set corresponding to the execution label that is a must be executed label, for each reachable state, based on the planning trajectory information corresponding to the reachable state and the evaluation factor corresponding to the reachable state , determine the evaluation index value corresponding to the reachable state;

Based on the evaluation index value corresponding to each reachable state, the target state corresponding to the target object is determined from all reachable states.

Optionally, the step of obtaining planning trajectory information corresponding to each reachable state in the set of reachable states includes:

Based on the path planning algorithm corresponding to each reachable state in the set of reachable states and the current surrounding perception information, current pose information, current map information and obstacle prediction information of the target object in the current decision planning related information, determine Planning trajectory information corresponding to each reachable state in the reachable state set.

Optionally, the method further includes:

A visual signal and visual information corresponding to the target state are determined based on the target state, and output, so that the target object is displayed accordingly based on the visual signal and the visual information.

In a second aspect, an embodiment of the present invention provides a path planning device, where the device includes:

The first obtaining module is configured to obtain the current decision planning related information corresponding to the target object;

The first determination module is configured to determine, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the relevant information of the current decision-making plan, and the preset driving limit condition, determine the available options corresponding to the target object. reaching a state set, the preset hierarchical state machine includes: the jump relationship and the hierarchical relationship between the states corresponding to the target object;

a second obtaining module, configured to obtain planning trajectory information corresponding to each reachable state in the set of reachable states;

The second determination module is configured to determine the target state corresponding to the target object from all the reachable states based on the planning trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state, wherein the target state corresponding to the target object can be determined. The level corresponding to the reached state is its level in the preset hierarchical state machine;

The control module is configured to control the preset hierarchical state machine to jump from the current state to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

Optionally, the current decision planning related information includes: current surrounding perception information, current pose information, current map information, and prediction information of obstacles of the target object.

Optionally, the first determination module is specifically configured to preprocess the specified information in the current decision planning related information, and determine whether each information in the current decision planning related information is valid;

In the case where it is determined that the obtained current decision planning related information is valid, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, A reachable state set corresponding to the target object is determined.

Optionally, the second determining module includes:

A judging unit, configured to judge whether there is a reachable state in which the corresponding execution label is a must-execute label in the reachable state set, wherein the jump label is: based on the current surrounding information in the current decision planning related information tags determined by sensory information and/or current map information;

The first determination unit is configured to, if it is determined that there is a reachable state in the reachable state set in which the corresponding execution label is a must be executed label, based on the current priority corresponding to the top-level state corresponding to each reachable state, determine the reachable state. The reachable state corresponding to the current highest priority is regarded as the state to be evaluated; based on the planning trajectory information corresponding to the state to be evaluated and the evaluation factors corresponding to the state to be evaluated, determine whether the planned trajectory information corresponding to the state to be evaluated is feasible. , the reachable state corresponding to the tag must be executed with the highest priority;

The second determining unit is configured to determine, based on the feasible planning trajectory information, the target state corresponding to the target object from the to-be-evaluated state if the evaluation result indicates that the planned trajectory information corresponding to the to-be-evaluated state is feasible;

If the evaluation result indicates that the planned trajectory information corresponding to the state to be evaluated is infeasible, the process returns to trigger the first determination unit.

Optionally, if the to-be-evaluated state includes reachable states that share a common parent state with each other and are at the same level; there is a temporal phase transition relationship between the reachable states that share a common parent state and are at the same level;

The first determining unit is specifically configured to, for the to-be-evaluated states that share a common parent state with each other and are at the same level, sequentially determine the currently unevaluated and the The first reachable state corresponding to the transition relationship in the time sequence stage is the current state to be evaluated;

Determine the evaluation index value corresponding to the current state to be evaluated based on the planning trajectory information corresponding to the current state to be evaluated and the evaluation factor corresponding to the current state to be evaluated;

If the rating index value corresponding to the current to-be-evaluated state represents: the planned trajectory information corresponding to the current to-be-evaluated state is feasible, determine whether there is an unevaluated state in the to-be-evaluated state;

If it is judged to exist, returning to the state to be evaluated, and determining the unevaluated and the first reachable state of the corresponding sequence stage transition relationship, as the step of the current state to be evaluated;

If it is judged that it does not exist, or it is determined that the planned trajectory information corresponding to the current state to be evaluated is infeasible, an evaluation result of whether the planned trajectory information corresponding to the state to be evaluated is feasible is obtained.

Optionally, the second determining unit is specifically configured to determine, among the reachable states corresponding to the feasible planning trajectory information, the last reachable state of the corresponding time sequence phase transition relationship as the target state corresponding to the target object. .

Optionally, the second determining module further includes:

The third determining unit is configured to, if it is determined that there is no reachable state in the reachable state set that corresponds to the execution label that is a must-execute label, for each reachable state, based on the planned trajectory information corresponding to the reachable state and The evaluation factor corresponding to the reachable state is determined, and the evaluation index value corresponding to the reachable state is determined;

Based on the evaluation index value corresponding to each reachable state, the target state corresponding to the target object is determined from all reachable states.

Optionally, the second obtaining module is specifically configured to be based on the path planning algorithm corresponding to each reachable state in the reachable state set and the current surrounding perception information of the target object in the current decision planning related information, the current The position and attitude information, the current map information and the prediction information of obstacles are used to determine the planned trajectory information corresponding to each reachable state in the set of reachable states.

Optionally, the device further includes:

A determining and outputting module is configured to determine a visual signal and visual information corresponding to the target state based on the target state, and output the visual signal, so that the target object can be displayed accordingly based on the visual signal and the visual information.

It can be seen from the above that a path planning method and device provided by the embodiments of the present invention obtain the current decision planning related information corresponding to the target object; based on the current state of the preset hierarchical state machine, the jump relationship between the states, The current decision-making planning related information and the preset driving conditions determine the set of reachable states corresponding to the target object. The preset hierarchical state machine includes: the jump relationship and hierarchical relationship between the states of the target object; obtain the set of reachable states planning trajectory information corresponding to each reachable state in the The level corresponding to the reachable state is the level in the preset hierarchical state machine; the preset hierarchical state machine is controlled to jump from the current state to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

Applying the embodiments of the present invention, it is possible to first determine the current state of the preset hierarchical state machine based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving restrictions. A set of reachable states to which a state can be jumped, that is, to determine the set of reachable states at the next moment of the target object corresponding to the preset hierarchical state machine, and then use the rationality of the planning trajectory information corresponding to each reachable state, Determine the most reasonable reachable state corresponding to the planning trajectory information from the set of reachable states, that is, the target state, realize the rational and accurate determination of the state of the target object at the next moment, and improve the rationality of the path planning. , and in the planning process, first determine the reachable state, and then obtain the planning trajectory information corresponding to the reachable state, so as to determine the target state from the reachable state through the evaluation of the planning information, the planning framework is more generalizable , scalable and interpretable, can quickly define and embed new scenarios and action modes for emerging new problems and scenarios, and adapt to new path planning algorithms. Of course, it is not necessary for any product or method of the present invention to achieve all of the advantages described above at the same time.

The innovative points of the embodiments of the present invention include:

1. First determine the reachable state set, and then analyze the evaluation results of the planning trajectory information corresponding to each reachable state in the reachable state set, and determine the most reasonable reachable state from the reachable state set, as the target state, to achieve The rational and accurate determination of the state of the target object at the next moment realizes the improvement of the rationality of the path planning. Moreover, in the planning process, the reachable state is first determined, and then the planning trajectory information corresponding to the reachable state is obtained, so that the target state can be determined from the reachable state through the evaluation of the planning information. The planning framework is more generalizable, Extensible and interpretable, it can quickly define and embed new scenarios and action modes for emerging new problems and scenarios, and adapt to new path planning algorithms.

2. First, determine the validity of each information in the current decision-making planning related information. When the current decision-making planning related information is valid, execute the subsequent path planning process to ensure the safety and rationality of the path planning.

3. When there is a reachable state in which the corresponding execution label is a must-execute label in the reachable state set, that is, there is a reachable state that must be executed. In this case, the current priority is used to give priority to the reachable state of the must-execute label. The evaluation of the reachable state is called arbitration. When it is determined that the corresponding planning trajectory information is feasible, the reachable state is directly determined as the target state, and the reachable state with the highest priority corresponding to the reachable state base of the must-execute label is determined. When the corresponding planning trajectory is not feasible, the arbitration will be performed in turn to ensure that the determined target state is more reasonable and more in line with the current environmental conditions.

4. In the case that there is no reachable state in the reachable state set whose corresponding execution label is a mandatory label, the evaluation index value corresponding to each reachable state can be determined in parallel, and the most reasonable reachable state can be determined as the target state.

5. For the sub-states that have a common parent state, set the transition relationship of the time sequence stage, and determine the evaluation index value corresponding to each sub-state in combination with the transition relationship of the time sequence stage, that is, determine the feasibility of the planning trajectory information corresponding to each sub-state, and pass the time sequence stage transition. relationship, which restricts the execution sequence of child states with a common parent state, and ensures the optimality and coherence of each state transition of the target object.

Description of drawings

In order to illustrate the embodiments of the present invention or the technical solutions in the prior art more clearly, the following briefly introduces the accompanying drawings that are required in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the invention. For those of ordinary skill in the art, other drawings can also be obtained from these drawings without creative effort.

FIG. 1 is a schematic flowchart of a path planning method provided by an embodiment of the present invention;

2A and 2B are a schematic diagram of a state jump logic of a preset hierarchical state machine;

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

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, but not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

It should be noted that the terms "comprising" and "having" and any modifications thereof in the embodiments of the present invention and the accompanying drawings are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the steps or units listed, but optionally also includes steps or units not listed, or optionally also includes For other steps or units inherent to these processes, methods, products or devices.

The present invention provides a path planning method and device to improve the rationality of path planning. The embodiments of the present invention will be described in detail below.

FIG. 1 is a schematic flowchart of a path planning method provided by an embodiment of the present invention. The method may include the following steps:

S101: Obtain current decision planning related information corresponding to the target object.

The path planning method provided by the embodiment of the present invention can be applied to any electronic device with computing capability, and the electronic device can be a terminal or a server. In one implementation, the functional software for implementing the path planning method may exist in the form of a separate client software, or may exist in the form of a plug-in of the currently related client software, for example, it may exist in the form of a functional module of an automatic driving system. Form exists, this is all possible.

The target object may be an object that needs path planning, such as an autonomous vehicle and a robot. In one implementation, when the target object is an autonomous vehicle, the electronic device implementing the path planning method may be an in-vehicle device, set on the target object, and can directly obtain current decision planning related information corresponding to the target object. In another case, the electronic device implementing the path planning method may also be an off-board device, and the corresponding electronic device may be connected to the data processing device corresponding to the target object to obtain the target object sent by the data processing device corresponding to the target object. Corresponding information about the current decision plan.

In an implementation manner of the present invention, the current decision planning related information may include, but is not limited to, current surrounding perception information, current pose information, current map information, and obstacle prediction information of the target object. The current map information may refer to map information corresponding to the environment where the target object is located. The predicted information of the obstacle may include, but is not limited to, the predicted travel trajectory of the obstacle and travel parameter information.

The current decision planning related information is information used to assist decision planning and path planning, wherein the decision planning is to determine the state to which the target object needs to jump to, and then the target object is based on the path corresponding to the state to be jumped to. information for driving and path planning. The current decision-making planning related information is: information determined based on sensor data collected by sensors set by the target object. The sensors set by the target object may include but are not limited to: image acquisition equipment, wheel speed sensors, radar, IMU ( Inertial measurement unit, Inertial Measurement Unit), GPS (Global Positioning System, Global Positioning System) and GNSS (Global Navigation Satellite System, Global Satellite Navigation System/Global Navigation Satellite System) and so on.

S102: Determine a set of reachable states corresponding to the target object based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision-making planning related information, and the preset driving limit conditions.

The preset hierarchical state machine includes: a jump relationship and a hierarchical relationship between states corresponding to the target object.

A preset hierarchical state machine is pre-stored locally on the electronic device or in a connected storage device, the preset hierarchical state machine includes various states corresponding to the corresponding target object, and the preset hierarchical state machine includes each state corresponding to the target object Jump relationship and hierarchical relationship between.

The preset hierarchical state machine includes states at the same level, and also includes states at different levels. Different states can represent different execution actions of the target object, that is, a certain state of a certain level in the preset hierarchical state machine can display a certain action mode representing the target object. The jump of the state of the preset hierarchical state machine can control the corresponding execution action of the target object, that is, the transition of the action mode.

The state of the next level corresponding to a state of a certain level may be referred to as a sub-state of the state, and the different sub-states corresponding to the state may be considered as the disassembly or stage division of the action mode corresponding to the state. For example, if the target object is an autonomous vehicle, as shown in FIG. 2A , the preset hierarchical state machine, that is, the “hierarchical state machine” described in FIG. 2A may include: lane keeping, lane changing and borrowing are at the first level That is, the state of the top level, wherein the lane change may include sub-states at the second level such as lane change preparation and lane change execution, and the lane change may also include a lane change return sub-state. Correspondingly, the preset hierarchical state machine may include two levels of states. For example, the preset hierarchical state machine includes: lane keeping, pullover and other states in the first level, wherein the pullover may include deceleration and pullover execution and other sub-states in the second level.

According to the different action modes of the target object, that is, the continuous division of the execution action, a preset hierarchical state machine including more levels of states can also be constructed. For example, the lane change execution in the sub-state at the second level may also include lower-level sub-states such as driving to the target lane, entering the target lane, and driving in the target lane, and the like.

It can be understood that it can be considered that the state at the lowest level in the preset hierarchical state machine corresponds to a specific subdivision scene. For example, based on the above example, the target object is in a scene that needs to be prepared for lane change, and the target object is in a scene that needs to be changed. The scene of lane execution, etc.; lane keeping does not correspond to the state of the next level, and the target object is in the scene that requires the target object to be in the lane keeping. It needs to perform complete trajectory planning and obtain the corresponding planned trajectory information.

The preset driving restriction condition is a condition that defines the driving mode of the target object. For example, the preset driving restriction condition may include the traffic rules to be complied with in the area where the target object is located, or may include the restriction condition corresponding to the task performed by the target object, or Artificially added empirical and generalized conditions can be included to limit the driving behavior of the target object. In one case, the artificially added empirical and general conditions include, but are not limited to: conditions for limiting the number of required calculation scenarios and constraints, and the number of scenarios is determined by the data of the reachable state. Sure. The execution tasks of the target object may include, but are not limited to: tasks that the target object needs to drive to various destinations; tasks that the target object needs to complete during the driving process, and the like.

In the embodiment of the present invention, the function of allowing manual addition of conditions can be retained, so that other conditions can be added and opened after the path planning capability boundary of the subsequent electronic device is expanded.

In order to plan a better path and plan a better decision, it is necessary to ensure the continuity of the determined series of states, that is, the execution of the target object. , in order to maintain the continuity of the action, the latter state should be a left turn, so as to avoid the problem that the target object turns left and right for a while when the target object is turning. Correspondingly, the reachable state set needs to be determined in combination with the current state of the preset hierarchical state machine.

In this step, the electronic device can detect the current state of the preset hierarchical state, and further, based on the current state of the preset hierarchical state machine and the jump relationship between the states, from each state of the preset hierarchical state machine The state that can be jumped from the current state is determined as a backup state, and then, based on the current decision-making planning related information and the preset driving limit The state of the decision-making planning related information is regarded as the reachable state corresponding to the target object, so as to obtain the reachable state set corresponding to the target object.

For example, the preset driving restrictions include: when the target object enters the line of sight area, it is forbidden to jump from lane keeping to lane change, and based on the current decision-making planning related information, it is determined that there is an accident vehicle in front of the target object or the blind area of the target object. A pedestrian who suddenly crosses quickly. In this case, in order to avoid the risk of collision, the target object's compaction line needs to violate the preset driving conditions to avoid the risk of collision, so as to ensure the safety of the target object, that is, the safety of other vehicles and pedestrians. , correspondingly, the lane change is determined as the reachable state at this time.

S103: Obtain planning trajectory information corresponding to each reachable state in the reachable state set.

The planned trajectory information includes but is not limited to: trajectory information that the target object needs to travel after jumping to the corresponding reachable state, travel parameter information of the target object, and relevant travel parameter information of other objects that have an interactive relationship with the target object .

The driving parameter information of the target object may include, but is not limited to, average speed information, acceleration information, and turning angle information. The relevant driving parameter information of other objects that have an interactive relationship with the target object may include, but not limited to, average speed information, acceleration information, and turning angle information.

Other objects that have an interactive relationship with the target object may refer to: the object that avoids the target object and/or the object that the target object avoids, that is, the object that is affected by the action performed by the target object, for example: when the target object changes lanes, it needs to change lanes To the object behind the target position of the target lane, the target position is the position where the target object's lane change is inserted.

In one case, after the electronic device determines the set of reachable states, it sends the set of reachable states, the current state, and current decision planning related information and preset driving conditions to other devices, so that the other devices can target each reachable state through the other devices. The state determines its corresponding planned trajectory information and sends it to the electronic device. Correspondingly, the electronic device obtains the planned trajectory information corresponding to each reachable state in the reachable state set.

In an implementation of the present invention, the preset hierarchical state and each state in the preset correspond to a corresponding path planning algorithm for planning a path. After the electronic device determines the reachable state set, it Reachable state, based on the path planning algorithm corresponding to the reachable state, the current surrounding perception information of the target object in the current decision planning related information, the current pose information, the current map information and the prediction information of the obstacles, determine the corresponding state of the reachable state. planning trajectory information.

S104: Determine the target state corresponding to the target object from all the reachable states based on the planning trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state.

The level corresponding to the reachable state is the level in the preset hierarchical state machine.

For each reachable state, the electronic device determines the evaluation index value corresponding to the reachable state based on the trajectory planning information corresponding to the reachable state, the level and evaluation factors corresponding to the reachable state, and further, based on the corresponding reachable state. The evaluation index value of , determines the most reasonable reachable state representing the corresponding trajectory planning information from all reachable states, as the target state corresponding to the target object.

Among them, when the evaluation index value exists in the form of a penalty item, that is, the lower the evaluation index value represents, the more reasonable the trajectory planning information corresponding to the reachable state is, the most reasonable reachable state may refer to the minimum value of the corresponding evaluation index value. reachable state; in the case that the evaluation index value exists in the form of a non-penalty item, that is, the higher the evaluation index value is, the more reasonable the trajectory planning information corresponding to the reachable state is, the most reasonable reachable state can refer to the corresponding evaluation index. The numerically largest reachable state of the value.

Among them, the process of determining the evaluation index value corresponding to each reachable state based on the trajectory planning information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state can be understood as constructing an arbitration tree for the set of reachable states The process of evaluating and arbitrating the planning trajectory information corresponding to each reachable state, after the construction of the arbitration tree is completed, that is, after the evaluation index value corresponding to each reachable state is determined, it can be determined based on the principle of depth first. The most reasonable target state corresponding to the final trajectory planning information is obtained, and the corresponding trajectory planning information is determined.

In one case, the evaluation factors may include, but are not limited to, factors such as the safety of the trajectory, comfort, traffic efficiency, preset driving conditions, and tasks performed by the target object.

Among them, in the evaluation and arbitration process of the planning trajectory information corresponding to the states of different levels, different combinations of evaluation factors are generally used to avoid the situation of local optimization. For the evaluation and arbitration of the low-level state, factors such as different avoidance decisions of specific obstacles, that is, safety, need to be considered, and the relevant factors such as the task performed by the target object and the scene environment in which it is located may not be considered; the evaluation and arbitration of the high-level state needs to be considered. Factors such as the task performed by the target object, the scene environment and the specific requirements of the traffic rules in the area.

In one case, the evaluation factors may include but are not limited to: the safety of the trajectory, the comfort of the trajectory, the traffic efficiency of the trajectory, and the preset execution penalty item corresponding to the trajectory.

Among them, the determination of the evaluation index value corresponding to the safety of the trajectory can be performed based on the planned trajectory in the planned trajectory information corresponding to the reachable state, and the predicted trajectory of other objects that have no interaction relationship with the target object to perform collision detection in the space-time dimension. Sure. It is necessary to consider the uncertainty of the predicted intention and the distribution of the trajectory points of the predicted trajectory of other objects that have no interaction with the target object to measure the probability information of collision of the planned trajectory in the planned trajectory information corresponding to the reachable state. It can be used in determining the evaluation and arbitration of different avoidance decisions for obstacles in determining the scene, for example: when the target object is in the lane-changing stage, select the insertion position to enter the target road to be changed, that is, which two objects in the target road should be inserted. used for gaps between.

Here, only objects that do not interact with the ego vehicle are considered in collision checking. It is further explained here that objects without interaction generally refer to vehicles whose own behavior and trajectory do not interfere, or whose own vehicle behavior does not affect the behavior of other vehicles (such as the vehicle in front of lane keeping), which needs to be comprehensively judged in combination with the scene; The object refers to the behavior of the ego vehicle (to complete the task or ensure safety) that will affect other cars, such as the vehicle behind the target lane when the ego car changes lanes. From an algorithmic point of view, targets that require interaction and those that do not require interaction will be screened based on empirical rules.

In one implementation, the rough mode or the precise mode can be used to set the safety threshold of the trajectory to determine whether the planned trajectory is safe and feasible, as expressed by the following formula (1):

Among them, J _safety represents the evaluation index value corresponding to the safety of the planned trajectory; p _collide represents the probability information of the collision of the planned trajectory in the planned trajectory information corresponding to the reachable state calculated in the precise mode; k _collide represents the predicted value in the precise mode. Set the weight coefficient; n _collide represents the information about whether the planned trajectory collides in the planned trajectory information corresponding to the reachable state in the rough mode.

The calculation of p _collide depends on the predicted information of obstacles in the current decision planning related information, wherein the predicted information of obstacles includes predicted trajectories of obstacles, wherein the predicted trajectories corresponding to each obstacle include multiple trajectory points; its calculation There are two ways. The first one is to sample the trajectory points in the predicted trajectory corresponding to the obstacle for each obstacle, and obtain the collision trajectory corresponding to the obstacle. The collision trajectory corresponding to the obstacle includes multiple collision trajectories. Sampling trajectory points; for each obstacle, use the sampled trajectory points at a certain moment in the collision trajectory corresponding to the obstacle and the trajectory points at the moment in the planned trajectory corresponding to the reachable state of the target object to perform point-by-point collision checking , count the number of collided sampling trajectory points corresponding to the obstacle; for each obstacle, use the number of collided sampling trajectory points corresponding to the obstacle and the total number of trajectory points to calculate the collision corresponding to the obstacle. The ratio of the number of trajectory points to the total number of trajectory points is used as the ratio corresponding to the obstacle; according to the ratio corresponding to all obstacles, the probability information of collision corresponding to the planned trajectory information corresponding to the reachable state is determined.

Among them, using the sampled trajectory points at a certain moment in the collision trajectory corresponding to the obstacle and the trajectory points at this moment in the planned trajectory corresponding to the reachable state of the target object, the process of performing point-by-point collision checking is as follows: according to a certain moment The pose information of the target object and the polygon model representing the target object, as well as the pose information of the obstacle and the polygon model representing the obstacle, determine whether the target object and the obstacle overlap at a certain moment, and the overlapping positions A collision occurs; otherwise, a collision does not occur if there is no overlapping of positions. The total number of trajectory points may refer to the total number of trajectory points in the planned trajectory corresponding to the reachable state of the target object, or may refer to the total number of trajectory points in the predicted trajectory obtained by using it.

The process of determining the probability information of collision corresponding to the planned trajectory information corresponding to the reachable state according to the ratios corresponding to all the obstacles may be: counting the number of obstacles whose corresponding ratios exceed the preset ratio threshold, based on the number, and the total number of obstacles, to determine the probability information of collision corresponding to the planned trajectory information corresponding to the reachable state.

The second: point by point according to the relative position of the trajectory point in the planned trajectory corresponding to the reachable state of the target object and the trajectory point in the predicted trajectory information of the obstacle, and the space of the trajectory point in the predicted trajectory information of the obstacle According to the probability distribution, the collision probability of the trajectory point is obtained; the collision probability of the trajectory point with the largest value is selected as the collision probability corresponding to the planned trajectory information corresponding to the reachable state of the target object.

Wherein, for the above process of obtaining the collision probability of the trajectory points, any method in the related art can be used to determine the collision probability of two trajectories colliding, for example: the spatial probability distribution of the trajectory points in the predicted trajectory information of the obstacle In the case of a discretized representation, the spatial probability distribution of the trajectory points in the predicted trajectory information of the obstacle and the corresponding weights of each pair of trajectory points can be directly used, and the trajectory points in the planned trajectory of the target object can be compared to determine the trajectory points. collision probability. When the spatial probability distribution of the trajectory points in the predicted trajectory information of the obstacle is continuous and integrable, the target object can be simplified as a circular model, and the trajectory points of the planned trajectory of the target object and the predicted trajectory of each obstacle can be determined. The collision between the trajectory points in the information, get the collision probability of the trajectory points, etc.

Among them, choosing to use the rough mode or the precise mode can be set according to the actual setting, such as the scene, and there is a corresponding relationship between the scene and the state.

The evaluation index value corresponding to the safety of the trajectory exists in the form of a penalty item. The larger the evaluation index value corresponding to the safety of the trajectory, the lower the safety of the trajectory corresponding to the planned trajectory information representing the reachable state.

The determination of the evaluation index value corresponding to the traffic efficiency of the trajectory needs to consider the average speed v of the planned trajectory in the planned trajectory information corresponding to the reachable state of the target object, and also needs to consider the ith object set Φ that has an interactive relationship with the target object. The average hourly speed change Δv _i generated by the object avoiding the target object; for objects with different priorities, it can be distinguished by the coefficient k _i , which is a preset value. Among them, in the evaluation index value corresponding to the traffic efficiency of the trajectory, the determination process of the evaluation index value part corresponding to each object in the object set Φ that has an interactive relationship with the target object can be expressed by the following formula (2):

J _interaction =∑ _i∈Φ -k _i Δv _i ; (2)

Among them, J _interaction represents the value of the evaluation index value part corresponding to each object in the object set Φ that has an interactive relationship with the target object in the evaluation index value corresponding to the traffic efficiency of the trajectory. Δv _i can be determined from the current surrounding perception information.

The evaluation index value corresponding to the traffic efficiency of the trajectory exists in the form of a penalty item. The larger the evaluation index value corresponding to the traffic efficiency of the trajectory, the lower the traffic efficiency of the trajectory corresponding to the planned trajectory information representing the reachable state.

In the determination of the evaluation index value corresponding to the comfort of the trajectory, on the one hand, the acceleration a corresponding to the planned trajectory in the planned trajectory information corresponding to the reachable state of the target object can be considered. Part of the value determination method can be expressed by the following formula (3);

J _comfort = k _comfort * a; (3)

Wherein, J _comfort represents the first part of the evaluation index values corresponding to the comfort of the trajectory, and k _comfort represents the coefficient value corresponding to the acceleration a, which is a preset value.

On the other hand, in the case of changing lanes or turning, the determination of the evaluation index value corresponding to the comfort of the trajectory may also consider the generated lateral acceleration a _lat . Correspondingly, the No. 1 evaluation index value corresponding to the comfort of the trajectory The two-part value determination method can be expressed by the following formula (4);

J _LCManeuver = k _LCManeuver *a _lat ; (4)

Among them, J _LCManeuver represents the second part of the evaluation index value corresponding to the comfort of the trajectory, and k _LCManeuver represents the coefficient value corresponding to the lateral acceleration a, which is a preset value.

The evaluation index value corresponding to the comfort of the trajectory exists in the form of a penalty item. The larger the evaluation index value corresponding to the comfort of the trajectory, the lower the comfort of the trajectory corresponding to the planned trajectory information representing the reachable state.

In the process of determining the evaluation index value corresponding to the planning trajectory information corresponding to the reachable state, n _LCNeed ∈ {0,1} can be used to indicate whether the execution label corresponding to the reachable state is a mandatory label, where n _LCNeed can be When it is 1, it means that the execution label corresponding to the reachable state is a must-execute label. When n _LCNeed is 1, it means that the execution label corresponding to the reachable state is not a must-execute label. It is determined according to whether the execution label corresponding to the reachable state is a must-execute label. Whether to increase the preset execution penalty item J _LCNeed corresponding to the trajectory.

S105: Control the preset hierarchical state machine to jump from the current state to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

After the electronic device determines the target state, it can control the preset hierarchical state machine to jump from the current state to the target state, and output the planned trajectory information corresponding to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

Applying the embodiment of the present invention, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, determine the next state from the preset hierarchical state machine. The set of reachable states that can be jumped to, that is, the set of reachable states at the next moment of the target object corresponding to the preset hierarchical state machine is determined, and then the rationality of the planning trajectory information corresponding to each reachable state is used to determine the set of reachable states corresponding to each reachable state. Determine the most reasonable reachable state of the corresponding planning trajectory information from the reachable state set, that is, the target state, realize the rational and accurate determination of the state of the target object at the next moment, and realize the improvement of the rationality of the path planning, and , in the planning process, first determine the reachable state, and then obtain the planning trajectory information corresponding to the reachable state, so as to determine the target state from the reachable state through the evaluation of the planning information. Extensible and interpretable, new scenarios and action modes can be quickly defined and embedded for emerging new problems and scenarios, and new path planning algorithms can be adapted.

In another embodiment of the present invention, the S102 may include the following steps 011-012:

011: Preprocess the specified information in the current decision planning related information, and judge whether each information in the current decision planning related information is valid.

012: In the case where it is determined that the obtained current decision planning related information is valid, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, Determine the set of reachable states corresponding to the target object.

In this implementation manner, in order to ensure the accuracy of subsequent decision-making results and the accuracy of path planning, after obtaining the current decision-making planning related information, the electronic device preprocesses the specified information in the current decision-making planning related information, for example: using the current pose information and its previous historical pose information to determine the driving trajectory of the target object; use the current surrounding perception information and historical surrounding perception information to determine the driving trajectory of each sensing target in the current surrounding perception information, and so on.

Further, based on the preprocessing result and the specific data of each information in the current decision planning related information, it is determined whether each information in the current decision planning related information is valid. For example: determine whether the current pose information is accurate based on the specific value of the current pose information in the current decision-making planning related information and the determined driving trajectory of the target object; The driving trajectory of each sensing target determines whether the current surrounding sensing information is accurate, whether there is a missed detection or false detection of the sensing target, and whether the position information of the sensing target is accurate. Whether the predicted information of the obstacle is accurate is determined based on the specific value of the predicted information of the obstacle.

When it is determined that all information related to the current decision planning is valid, the subsequent path planning process is performed. In order to ensure the accuracy of subsequent decision-making results and the accuracy of path planning.

In another implementation manner, when it is determined that the information related to the current decision-making plan is not all valid, the electronic device can determine the deceleration trajectory information based on the valid information in the current decision-making plan-related information, so as to control the target object to decelerate based on the information. Track information to decelerate. And reset the state of the preset hierarchical state machine. For example, the preset hierarchical state and the state of the state machine can be reset to their designated state, so as to ensure the safety of the target object and other objects that have an interactive relationship with it.

In one case, when the target object is an automatic driving vehicle, after the automatic driving system of the target object is activated, the electronic device can run at a fixed frequency. The specified information in the decision planning related information is preprocessed, and it is judged whether each information in the current decision planning related information is valid.

In another embodiment of the present invention, the S104 may include the following steps 021-024:

021: Determine whether there is a reachable state in which the corresponding execution label is a must-execute label in the reachable state set.

The jump label is: a label determined based on the current surrounding perception information and/or the current map information in the current decision planning related information.

022: If it is determined that there is a reachable state in the reachable state set in which the corresponding execution label is a mandatory label, based on the current priority corresponding to the top-level state corresponding to each reachable state, determine the reachable state with the highest current priority. The state is regarded as the state to be evaluated; based on the planned trajectory information corresponding to the to-be-evaluated state and the evaluation factors corresponding to the to-be-evaluated state, determine whether the planned trajectory information corresponding to the to-be-evaluated state is feasible or not.

Among them, the reachable state corresponding to the label that must be executed has the highest priority.

023: If the evaluation result indicates that the planned trajectory information corresponding to the state to be evaluated is feasible, then based on the feasible planned trajectory information, the target state corresponding to the target object is determined from the state to be evaluated.

024: If the evaluation result indicates that the planning trajectory information corresponding to the state to be evaluated is infeasible, return to 022.

When determining the next state of the target object, that is, determining the planned trajectory of the target object, priority should be given to the safety of the target object and other objects that have an interactive relationship with it. Information and the surrounding situation represented by the current map information inevitably require the target object to perform certain actions to avoid danger to the target object and/or other objects that have an interactive relationship with the target object. Correspondingly, there is inevitably a distinction between mandatory execution and non-essential execution in the determined reachable state. In order to ensure the security of the target object and other objects with which there is an interactive relationship, there is a difference between the evaluation and arbitration process of the planning trajectory information corresponding to the reachable state that must be executed and the reachable state that does not exist.

Wherein, whether the reachable state must be executed can be determined by the execution tag corresponding to the reachable state. Wherein, the execution label corresponding to the reachable state is: a label determined based on the current surrounding perception information and/or the current map information in the current decision planning related information. For example, when it is determined that there is a faulty object or a fast-moving object suddenly appearing in the lane ahead of the target object according to the current surrounding perception information, the target object needs to change lanes; When there is no road or lane in front of the vehicle, the target object needs to change lanes or turn; through the current surrounding perception information and current map information, it is determined that there is an intersection in front of the target object, and the task performed by the target object requires the target object Turn left, correspondingly, the target object needs to turn and drive.

In this implementation manner, in the process of evaluating and arbitrating the planned trajectory information corresponding to each reachable state, the electronic device firstly judges whether there is a reachable state in the reachable state set that corresponds to the execution label that must be executed. There is a reachable state in the reachable state set that the corresponding execution label is a must-be-executed label, and arbitration needs to be prioritized. If the planned trajectory information corresponding to the reachable state corresponding to the must-execute label is safe and feasible, priority is given to jumping to must. The reachable state of the execution tag.

The reachable state in the reachable state set can include the state at the top level or the state at the non-top level. If it is determined that there is a reachable state in the reachable state set with the corresponding execution label as the must-execute label , based on the priority status of the current priority corresponding to the state of the top level corresponding to the reachable state, the evaluation and arbitration can be performed for the planning trajectory information corresponding to the reachable state in turn. It is understandable that the current priority of the reachable state of the label must be executed. level is the highest.

Specifically, based on the current priority corresponding to the top-level state corresponding to each reachable state, the corresponding reachable state with the highest current priority is determined as the to-be-evaluated state; based on the planning trajectory information corresponding to the to-be-evaluated state and the to-be-evaluated state Corresponding evaluation factors, determine the evaluation index value corresponding to the state to be evaluated, and determine the evaluation result of whether the planning trajectory information corresponding to the state to be evaluated is feasible based on the evaluation index value corresponding to the state to be evaluated; If the planned trajectory information is feasible, the current reachable state is directly determined as the target state corresponding to the target object; there is no need to perform the evaluation arbitration process for the planned trajectory information corresponding to other reachable states with low current priority.

If the evaluation result indicates that the planned trajectory information corresponding to the to-be-evaluated state is infeasible, continue to perform evaluation arbitration on the planned trajectory information corresponding to the other reachable states with the current low priority, that is, return to execute the execution based on the top-level state corresponding to each reachable state. For the corresponding current priority, determine the reachable state with the highest corresponding current priority as the step to be evaluated.

In one case, if there is at least one planned trajectory information corresponding to the to-be-evaluated state in the planned trajectory information corresponding to the to-be-evaluated state, it can be determined that the planned trajectory information corresponding to the to-be-evaluated state is feasible.

The top-level state corresponding to the reachable state is: the state to which the reachable state belongs is at the top-level in the preset hierarchical state machine. For example, the states of the top level of the preset hierarchical state machine include lane keeping, lane change and lane borrowing; wherein, the state change of the top level includes three sub-states of lane change preparation, lane change execution and lane change return. In one case, the electronic device determines the set of reachable states corresponding to the target object based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving restriction conditions, including the following: Lane keeping, lane change preparation and lane change execution are three reachable states. Among them, lane keeping is at the top level, and its corresponding top level state is itself; lane change preparation and lane change execution are sub-states of lane change, and correspondingly, the corresponding top level state of lane change preparation and lane change execution for lane change.

In another embodiment of the present invention, there may be one or more states to be evaluated. For example, following the above example, the states of the top level corresponding to lane change preparation and lane change execution are both the top level state lane change, correspondingly, The current priority corresponding to the lane change preparation and the lane change execution are the same. The lane change preparation and the lane change execution correspond to the same parent state and are at the same level. It can be considered that the lane change preparation and the lane change execution have a common parent state and are in the same parent state. The reachability status of the hierarchy.

During the driving process, when the target object performs a certain series of execution actions, theoretically, there is a corresponding execution sequence between the series of execution actions. For example, when the target object needs to perform a lane change, if there are many When the target object performs a lane change, it is necessary to first determine the position where the lane change needs to be inserted, that is, the gap between two objects in the target road or the front or back of which object, and then drive on the road to which it is located. Determine the vicinity of the position to be inserted, and perform a lane change, that is, enter the target road.

In order to ensure the coherence and comfort of the execution of the target object, if the state to be evaluated includes reachable states that share a common parent state with each other and are at the same level, there is a temporal phase transition relationship between such states, and the temporal phase transition Relationships are used to constrain the order of execution between states.

Correspondingly, in an embodiment of the present invention, after determining the state to be evaluated, the electronic device determines that the state to be evaluated includes a plurality of situations, if the state to be evaluated includes mutually shared parent states and is at the same level reachable state; there is a time-series phase transition relationship between reachable states that have a common parent state and are at the same level.

The 022 may include the following steps 0221-0225:

0221: For the to-be-evaluated states that share a common parent state and are at the same level, based on the sequence of the transitional relationships of the time-series stages corresponding to the states to be evaluated, determine in turn the ones that are not currently evaluated and whose corresponding time-series-stage transition relationships are the first. Arrived status, which is the current status to be evaluated.

0222: Determine an evaluation index value corresponding to the current state to be evaluated based on the planning trajectory information corresponding to the current state to be evaluated and the evaluation factor corresponding to the current state to be evaluated.

0223: If the rating index value corresponding to the current to-be-evaluated state indicates that the planned trajectory information corresponding to the current to-be-evaluated state is feasible, determine whether there is an unevaluated state in the to-be-evaluated state.

0224: If it is judged to exist, return 0221.

0225: If it is judged that it does not exist, or it is determined that the planned trajectory information corresponding to the current state to be evaluated is infeasible, obtain an evaluation result of whether the planned trajectory information corresponding to the state to be evaluated is feasible.

In this implementation manner, the electronic device traverses the to-be-evaluated states in the to-be-evaluated states that share common parent states and are at the same level, and sequentially determines the currently unevaluated and The first reachable state of the transition relationship in the corresponding time sequence stage is the current state to be evaluated; based on the planning trajectory information corresponding to the current state to be evaluated and the evaluation factors corresponding to the current state to be evaluated, the evaluation corresponding to the current state to be evaluated is determined Index value. Further, based on the evaluation index value corresponding to the current state to be evaluated, it is judged whether the planning trajectory information corresponding to the current state to be evaluated is feasible, if the rating index value corresponding to the current state to be evaluated represents: the planned trajectory information corresponding to the current state to be evaluated Feasible, judge whether there is an unevaluated state in the state to be evaluated, and if so, continue to execute 0224; if it is judged that it does not exist, or it is determined that the planning trajectory information corresponding to the current state to be evaluated is not feasible, obtain the plan corresponding to the state to be evaluated The evaluation result of whether the trajectory information is feasible.

Wherein, if it is determined that the planned trajectory information corresponding to at least one state in the to-be-evaluated state is feasible, the determined evaluation result of whether the planned trajectory information corresponding to the to-be-evaluated state is feasible includes: information indicating that the planned trajectory information corresponding to the to-be-evaluated state is feasible , and carries the identifier of the feasible state of the planned trajectory information. If it is determined that the planned trajectory information corresponding to all states in the to-be-evaluated state is infeasible, the determined evaluation result of whether the planned trajectory information corresponding to the to-be-evaluated state is feasible includes: information indicating that the planned trajectory information corresponding to the to-be-evaluated state is infeasible.

The planned trajectory information corresponding to the sub-state corresponding to the top-level state is the sub-information of the planned trajectory information corresponding to the top-level state, that is, the planned trajectory information corresponding to the sub-state corresponding to the top-level state can be combined to form the planned trajectory corresponding to the top-level state. information. If the sub-state corresponding to the top level state also has a state of the next level, the planned trajectory information corresponding to each state of the next level of the sub-state can be combined to form the planned trajectory information corresponding to the sub-state. And so on. The planned trajectory information corresponding to each state that has a common parent state and is at the same level can be combined to form the planned trajectory information corresponding to its parent state.

For example, the states of the top level of the preset hierarchical state machine include lane keeping, lane change and lane borrowing; wherein, the state of the top level lane change includes three sub-states of lane change preparation, lane change execution and lane change return. The current state of the target object is lane keeping or lane change preparation. Based on the current priority corresponding to the top-level state corresponding to each reachable state, the reachable state with the highest current priority is determined as the state to be evaluated. The evaluation state includes lane change preparation, lane change execution and lane change return; among them, as shown in Figure 2B, the transition relationship between the time series stages between lane change preparation, lane change execution and lane change return is characterized: lane change preparation can transition to lane change Lane change execution, lane change execution can transition to lane change return, lane change return can transition to lane change execution, and lane change execution is completed, the lane change is completed.

In the case where the current state of the target object is lane keeping or lane change preparation, based on the sequence of the transition relationships of the time series stages corresponding to the states to be evaluated, the feasibility of the planned trajectory information corresponding to the arbitration lane change preparation can be evaluated in turn. The feasibility of executing the corresponding planned trajectory information and the feasibility of returning the corresponding planned trajectory information by changing lanes until the planned trajectory information corresponding to the evaluated arbitration state is infeasible, and determining whether the planned trajectory information corresponding to the state to be evaluated is feasible. result.

When the current state of the target object is lane change execution, based on the sequence of the transition relationships of the time series stages corresponding to the states to be evaluated, the feasibility of the planned trajectory information corresponding to the arbitration lane change execution and the corresponding lane change return can be evaluated in turn. The feasibility of the planned trajectory information is determined until there is an evaluation result of whether the planned trajectory information corresponding to the evaluated arbitration state is infeasible, and whether the planned trajectory information corresponding to the to-be-evaluated state is feasible.

In the case where the current state of the target object is lane change and return, based on the sequence of the transition relationships of the time series stages corresponding to the states to be evaluated, the feasibility of the arbitration lane change execution and lane change return can be evaluated in turn, until there is an evaluated arbitrator. The planned trajectory information corresponding to the state is infeasible, and the evaluation result of determining whether the planned trajectory information corresponding to the state to be evaluated is feasible.

As shown in FIG. 2A , if the current state of the preset hierarchical state machine, that is, the current state of the target object is lane keeping, the electronic device is based on the current state of the preset hierarchical state machine, the jump relationship between the states, and the current decision-making process. Planning-related information and preset driving constraints determine the set of reachable states corresponding to the target object. The “feasible target state set” shown in FIG. 2A includes: lane keeping, lane change preparation, and lane change execution, that is, preset points. The state of the layer state machine can be represented by lane keeping jump to lane keeping, or lane keeping jumping to lane change preparation, or lane keeping jumping to lane change execution. The electronic device can obtain the corresponding planned trajectory information of each reachable state in the reachable state set; based on the planned trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state, determine the The evaluation index value is determined, and based on the evaluation index value corresponding to the reachable state, the target state corresponding to the target object is determined from the reachable state, that is, an arbitration tree is constructed.

The electronic device can build an arbitration tree from the bottom up, that is, determine the evaluation index value corresponding to each reachable state. Correspondingly, since the lane change preparation and the lane change execution have a common parent state and are in the same level state, as shown in Figure 2A As shown, firstly, based on the planning trajectory information corresponding to the lane change preparation, the level and evaluation factors corresponding to the lane change preparation, determine the evaluation index value corresponding to the lane change preparation; based on the evaluation index value corresponding to the lane change preparation, the planning corresponding to the lane change preparation is determined. The trajectory information is evaluated and arbitrated. If the evaluation index value corresponding to the lane change preparation indicates that the planned trajectory information corresponding to the lane change preparation is feasible, then the lane change is determined based on the planned trajectory information corresponding to the lane change execution, the level and evaluation factors corresponding to the lane change execution. Execute the corresponding evaluation index value, and evaluate and arbitrate the planning trajectory information corresponding to the lane change execution based on the evaluation index value corresponding to the lane change execution. In one case, if the execution tag corresponding to the lane change is a mandatory execution tag, the corresponding electronic device may directly determine the lane change preparation as the target state. In another case, if there is no state in the reachable state set that the corresponding execution label is a must-execute label, then the evaluation index corresponding to the lane keeping is determined based on the planning trajectory information corresponding to the lane keeping, the level and evaluation factors corresponding to the lane keeping Based on the evaluation index value corresponding to lane keeping and the evaluation index value corresponding to lane change preparation, determine the optimal planning trajectory information among the planning trajectory information corresponding to lane keeping and the planning trajectory information corresponding to lane changing preparation, and assign the optimal planning trajectory information to the planning trajectory information corresponding to lane changing preparation. The reachable state corresponding to the trajectory information is determined as the target state.

Correspondingly, in another embodiment of the present invention, the 023 includes the following steps:

Among the reachable states corresponding to the feasible planning trajectory information, the last reachable state of the corresponding time sequence phase transition relationship is determined as the target state corresponding to the target object.

In this implementation manner, if the evaluation result corresponding to the to-be-evaluated state indicates that the planned trajectory information corresponding to the to-be-evaluated state is feasible, then based on the evaluation result corresponding to the to-be-evaluated state, the identification of the reachable state corresponding to the feasible planned trajectory information is determined, and the Among the reachable states corresponding to the feasible planning trajectory information, the last reachable state of the transition relationship in the corresponding time sequence stage is determined as the target state corresponding to the target object. For example, following the above example, if the feasibility of preparing the corresponding planned trajectory information for lane change, the feasibility of executing the corresponding planned trajectory information in the lane change, and the feasibility of returning the corresponding planned trajectory information from the lane change are evaluated and arbitrated in turn, wherein, If it is determined that the planned trajectory information corresponding to the lane change preparation is feasible, but the planned trajectory information corresponding to the lane change execution is not feasible, the lane change preparation is determined to be the target state corresponding to the target object. After the target state is determined, the electronic device can control the preset hierarchical state machine to maintain lane from the current state and jump to the target state, such as lane change preparation, based on the state transition function.

As shown in FIG. 2A , the preset hierarchical state machine can support the scene configuration of the staff, so that the preset hierarchical state machine can expand the state and/or modify the state according to the actual usage scenario.

In another embodiment of the present invention, the S104 may include the following steps 025-026:

025: If it is determined that there is no reachable state in the reachable state set that corresponds to the execution label and the label must be executed, for each reachable state, based on the planning trajectory information corresponding to the reachable state and the evaluation factor corresponding to the reachable state , and determine the evaluation index value corresponding to the reachable state.

026: Based on the evaluation index value corresponding to each reachable state, determine the target state corresponding to the target object from all reachable states.

In this implementation manner, if the electronic device determines that there is no reachable state in the reachable state set corresponding to the execution label, which is a reachable state that must execute the label, and if each reachable state belongs to the same level, it can be considered that the selection level between each reachable state is is equal, correspondingly, the electronic device can, for each reachable state, determine the evaluation index value corresponding to the reachable state based on the planning trajectory information corresponding to the reachable state and the evaluation factor corresponding to the reachable state; and further, based on The evaluation index value corresponding to each reachable state determines a relatively safe and comfortable and reasonable reachable state from all reachable states as the target state corresponding to the target object.

In another implementation of the present invention, if the electronic device determines that there is no corresponding execution tag in the reachable state set, it is a reachable state that must execute the tag, and there are states belonging to different levels in each reachable state. There are states that share a common parent state with each other and are at the same level. For the states that share a common parent state and are at the same level, when determining the feasibility of planning trajectory information corresponding to the states that share a common parent state and are at the same level, that is, when evaluating the arbitration process, it is necessary to combine the mutual consideration. It is a temporal phase transition relationship between states that have a common parent state and are at the same level.

If it is determined that there is feasible planning trajectory information from the planning trajectory information corresponding to the states that have common parent states and are at the same level, return the corresponding planning trajectory information that is feasible and that corresponds to the last state of the transition relationship of the corresponding time sequence stage. The evaluation index value is used as the first evaluation index value, and the first evaluation index value is compared with the evaluation index value corresponding to the parent state that has a common parent state and is at the same level, so as to determine the corresponding plan. The optimal state of the trajectory information is taken as the target state. Among them, if the evaluation index value exists in the form of a penalty item, the larger the evaluation index value corresponding to the reachable state is, the more unreasonable the corresponding planned trajectory information is. Correspondingly, the optimal state of the corresponding planned trajectory information can be: The state with the smallest value of the corresponding evaluation index.

In another embodiment of the present invention, the method may further include:

Based on the target state, a visual signal and visual information corresponding to the target state are determined and output, so that the target object is displayed accordingly based on the visual signal and the visual information.

In this implementation, it is considered that the target object needs to show the state it will be in and the state it will be in to its corresponding user and other objects that have an interactive relationship with it in the process of executing the driving process and executing the action corresponding to each state. Actions. Correspondingly, after determining the target state, the electronic device can determine and output the visual signal and visual information corresponding to the target state based on the target state, so that the target object can be displayed accordingly based on the visual signal and visual information.

The visual signal may be information that is determined based on the target state and is displayed by a designated display device used to control the target object. For example, the target state is a lane change, and the visual signal is an indication of a left turn or a right turn for controlling the target object. A signal that the light flashes or lights up continuously, wherein the specific left turn indicator light or right turn indicator light is on is determined based on the planned trajectory information corresponding to the target state. Visualization information is the information that is displayed to the user of the target object, for example, it can be information used to describe the state that the target object is about to jump to, that is, the decision result, and/or information used to describe the action to be performed by the target object, etc. .

Corresponding to the foregoing method embodiments, an embodiment of the present invention provides a path planning apparatus. As shown in FIG. 3 , the apparatus may include:

The first obtaining module 310 is configured to obtain the current decision planning related information corresponding to the target object;

The first determination module 320 is configured to determine the corresponding target object based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions. A set of reachable states, the preset hierarchical state machine includes: a jump relationship and a hierarchical relationship between states corresponding to the target object;

The second obtaining module 330 is configured to obtain planning trajectory information corresponding to each reachable state in the set of reachable states;

The second determination module 340 is configured to determine the target state corresponding to the target object from all the reachable states based on the planning trajectory information corresponding to each reachable state, the level and evaluation factors corresponding to each reachable state, wherein, The level corresponding to the reachable state is its level in the preset hierarchical state machine;

The control module 350 is configured to control the preset hierarchical state machine to jump from the current state to the target state, so that the target object travels based on the planned trajectory information corresponding to the target state.

Applying the embodiment of the present invention, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, determine the next state from the preset hierarchical state machine. The set of reachable states that can be jumped to, that is, the set of reachable states at the next moment of the target object corresponding to the preset hierarchical state machine is determined, and then the rationality of the planning trajectory information corresponding to each reachable state is used to determine the set of reachable states corresponding to each reachable state. Determine the most reasonable reachable state of the corresponding planning trajectory information from the reachable state set, that is, the target state, realize the rational and accurate determination of the state of the target object at the next moment, and realize the improvement of the rationality of the path planning, and , in the planning process, first determine the reachable state, and then obtain the planning trajectory information corresponding to the reachable state, so as to determine the target state from the reachable state through the evaluation of the planning information. Extensible and interpretable, new scenarios and action modes can be quickly defined and embedded for emerging new problems and scenarios, and new path planning algorithms can be adapted.

In another embodiment of the present invention, the current decision planning related information includes: current surrounding perception information, current pose information, current map information, and prediction information of obstacles of the target object.

In another embodiment of the present invention, the first determining module 320 is specifically configured to preprocess the specified information in the current decision planning related information, and determine each item in the current decision planning related information whether the information is valid;

In the case where it is determined that the obtained current decision planning related information is valid, based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision planning related information, and the preset driving limit conditions, A reachable state set corresponding to the target object is determined.

In another embodiment of the present invention, the second determining module 340 includes:

A judging unit (not shown in the figure), configured to judge whether there is a reachable state in which the corresponding execution label is a must-execute label in the reachable state set, wherein the jump label is: based on the current decision The label determined by the current surrounding perception information and/or the current map information in the planning-related information;

The first determination unit (not shown in the figure) is configured to, if it is determined that there is a reachable state in the reachable state set that corresponds to the execution label as a must-executed label, based on the corresponding top-level state corresponding to each reachable state. The current priority corresponding to the current priority is determined as the state to be evaluated; based on the planning trajectory information corresponding to the state to be evaluated and the evaluation factors corresponding to the state to be evaluated, the planned trajectory information corresponding to the state to be evaluated is determined. Whether the evaluation result is feasible, among which, the reachable state corresponding to the label must be executed with the highest priority;

The second determination unit (not shown in the figure) is configured to determine the target object from the to-be-evaluated state based on the feasible planned trajectory information if the evaluation result indicates that the planned trajectory information corresponding to the to-be-evaluated state is feasible the corresponding target state;

If the evaluation result indicates that the planned trajectory information corresponding to the state to be evaluated is infeasible, the process returns to trigger the first determination unit.

In another embodiment of the present invention, if the states to be evaluated include reachable states that share a common parent state with each other and are at the same level; there is a time sequence stage between the reachable states that share a common parent state and are at the same level transitional relationship;

The first determination unit (not shown in the figure) is specifically configured to, for the to-be-evaluated states that have a common parent state and are at the same level, based on the sequence of the transitional relationships of the time-series stages corresponding to the to-be-evaluated states, in turn. Determine the reachable state that is not currently evaluated and the first transition relationship of the corresponding time sequence phase is the current state to be evaluated;

Determine the evaluation index value corresponding to the current state to be evaluated based on the planning trajectory information corresponding to the current state to be evaluated and the evaluation factor corresponding to the current state to be evaluated;

If the rating index value corresponding to the current to-be-evaluated state represents: the planned trajectory information corresponding to the current to-be-evaluated state is feasible, determine whether there is an unevaluated state in the to-be-evaluated state;

If it is judged to exist, returning to the state to be evaluated, and determining the unevaluated and the first reachable state of the corresponding time sequence phase transition relationship, as the step of the current state to be evaluated;

If it is judged that it does not exist, or it is determined that the planned trajectory information corresponding to the current state to be evaluated is infeasible, an evaluation result of whether the planned trajectory information corresponding to the state to be evaluated is feasible is obtained.

In another embodiment of the present invention, the second determining unit (not shown in the figure) is specifically configured to determine the last available state of the transition relationship of the corresponding timing stage in the reachable state corresponding to the feasible planning trajectory information. reach state, and determine the target state corresponding to the target object.

In another embodiment of the present invention, the second determining module 340 further includes:

The third determining unit is configured to, if it is determined that there is no reachable state in the reachable state set that corresponds to the execution label that is a must-execute label, for each reachable state, based on the planned trajectory information corresponding to the reachable state and The evaluation factor corresponding to the reachable state is determined, and the evaluation index value corresponding to the reachable state is determined;

Based on the evaluation index value corresponding to each reachable state, the target state corresponding to the target object is determined from all reachable states.

In another embodiment of the present invention, the second obtaining module 330 is specifically configured to be based on the path planning algorithm corresponding to each reachable state in the reachable state set and the target object in the current decision planning related information The current surrounding perception information, the current pose information, the current map information and the prediction information of the obstacle are determined, and the planned trajectory information corresponding to each reachable state in the set of reachable states is determined.

In another embodiment of the present invention, the device further includes:

A determination output module (not shown in the figure) is configured to determine, based on the target state, a visual signal and visual information corresponding to the target state, and output it, so that the target object performs an operation based on the visual signal and the visual information. corresponding display.

The foregoing system and device embodiments correspond to the system embodiments, and have the same technical effects as the method embodiments. For specific descriptions, refer to the method embodiments. The apparatus embodiment is obtained based on the method embodiment, and the specific description can refer to the method embodiment section, which will not be repeated here. Those of ordinary skill in the art can understand that the accompanying drawing is only a schematic diagram of an embodiment, and the modules or processes in the accompanying drawing are not necessarily necessary to implement the present invention.

Those skilled in the art may understand that: the modules in the apparatus in the embodiment may be distributed in the apparatus in the embodiment according to the description of the embodiment, and may also be located in one or more apparatuses different from this embodiment with corresponding changes. The modules in the foregoing embodiments may be combined into one module, or may be further split into multiple sub-modules.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that it can still be The technical solutions described in the foregoing embodiments are modified, or some technical features thereof are equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions in the embodiments of the present invention.

Claims (10)

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

obtaining relevant information of a current decision-making plan corresponding to a target object;

determining a reachable state set corresponding to the target object based on a current state of a preset layered state machine, a jump relation among states, the current decision-making planning related information and a preset driving limiting condition, wherein the preset layered state machine comprises: jump relations and hierarchical relations among the states corresponding to the target object;

acquiring planning track information corresponding to each reachable state in the reachable state set;

determining a target state corresponding to the target object from all reachable states based on planning track information corresponding to each reachable state, a level corresponding to each reachable state and evaluation factors, wherein the level corresponding to each reachable state is the level of the reachable state in a preset layered state machine;

and controlling the preset hierarchical state machine to jump from the current state to the target state so as to enable the target object to run based on the planned track information corresponding to the target state.

2. The method of claim 1, wherein the current decision plan related information comprises: current surrounding perception information, current pose information, current map information, and prediction information of the obstacle of the target object.

3. The method as claimed in claim 1, wherein the step of determining the reachable state set corresponding to the target object based on the current state of the preset hierarchical state machine, the jump relationship between the states, the current decision-making planning related information and the preset driving restriction condition comprises:

preprocessing the specified information in the current decision-making planning related information, and judging whether each information in the current decision-making planning related information is effective or not;

and under the condition that the obtained relevant information of the current decision-making plan is effective, determining a reachable state set corresponding to the target object based on the current state of a preset hierarchical state machine, the jump relation among the states, the relevant information of the current decision-making plan and a preset running limiting condition.

4. The method according to any one of claims 1 to 3, wherein the step of determining the target state corresponding to the target object from all reachable states based on the planned trajectory information corresponding to each reachable state, the hierarchy corresponding to each reachable state, and the evaluation factor comprises:

judging whether the reachable state set has a reachable state in which the corresponding execution tag is the necessary execution tag, wherein the skip tag is: a label determined based on current surrounding awareness information and/or current map information in the current decision plan related information;

if the reachable state set is determined to have the corresponding execution tag as the reachable state of the tag to be executed, determining the reachable state with the highest corresponding current priority as the state to be evaluated based on the current priority corresponding to the top level state corresponding to each reachable state; determining whether the planning track information corresponding to the state to be evaluated is feasible or not based on the planning track information corresponding to the state to be evaluated and the evaluation factor corresponding to the state to be evaluated, wherein the reachable state corresponding to the execution tag has the highest priority;

if the evaluation result represents that the planning track information corresponding to the state to be evaluated is feasible, determining a target state corresponding to the target object from the state to be evaluated based on the feasible planning track information;

and if the evaluation result represents that the planning track information corresponding to the state to be evaluated is not feasible, returning to execute the current priority corresponding to the top level state corresponding to each reachable state, and determining the reachable state with the highest corresponding current priority as the state to be evaluated.

5. The method of claim 4, wherein if the states to be evaluated include reachable states that are common parent states to each other and are in the same level; the reachable states which have public father states and are in the same level have a time sequence stage transition relation;

the step of determining whether the planning track information corresponding to the state to be evaluated is feasible or not based on the planning track information corresponding to the state to be evaluated and the evaluation factor corresponding to the state to be evaluated includes:

aiming at to-be-evaluated states which have public father states and are in the same level, sequentially determining a reachable state which is not evaluated currently and has the first transition relation of the corresponding time sequence stage as the current to-be-evaluated state based on the sequence of the transition relation of the time sequence stage corresponding to each to-be-evaluated state;

determining an evaluation index value corresponding to the current state to be evaluated based on the planning track information corresponding to the current state to be evaluated and the evaluation factor corresponding to the current state to be evaluated;

if the rating index value corresponding to the current state to be evaluated represents: the planning track information corresponding to the current state to be evaluated is feasible, and whether an unevaluated state exists in the state to be evaluated is judged;

if yes, returning to the slave state to be evaluated, and determining an unacevated reachable state with the first transition relation of the corresponding time sequence stage as the current state to be evaluated;

and if the evaluation result does not exist, or the planning track information corresponding to the current state to be evaluated is determined to be not feasible, obtaining the evaluation result whether the planning track information corresponding to the state to be evaluated is feasible or not.

6. The method according to claim 5, wherein the step of determining the target state corresponding to the target object from the states to be evaluated based on the feasible planning trajectory information comprises:

and determining the reachable state of the transition relation of the corresponding time sequence stage in the reachable states corresponding to the feasible planning track information as the target state corresponding to the target object.

7. The method of claim 4, wherein the method further comprises:

if it is determined that the corresponding execution tag does not exist in the reachable state set and is the reachable state in which the tag needs to be executed, determining an evaluation index value corresponding to the reachable state based on the planning track information corresponding to the reachable state and the evaluation factor corresponding to the reachable state for each reachable state;

and determining the target state corresponding to the target object from all reachable states based on the evaluation index value corresponding to each reachable state.

8. The method of claim 1, wherein the step of obtaining planned trajectory information corresponding to each reachable state in the set of reachable states comprises:

and determining planning track information corresponding to each reachable state in the reachable state set based on a path planning algorithm corresponding to each reachable state in the reachable state set and current surrounding perception information, current pose information, current map information and prediction information of the barrier of the target object in the current decision planning related information.

9. The method of any one of claims 1-8, further comprising:

and determining a visualization signal and visualization information corresponding to the target state based on the target state, and outputting the visualization signal and visualization information to enable the target object to be correspondingly displayed based on the visualization signal and visualization information.

10. A path planning apparatus, the apparatus comprising:

a first obtaining module configured to obtain current decision-making planning related information corresponding to a target object;

a first determining module, configured to determine, based on a current state of a preset hierarchical state machine, a skip relationship between states, the current decision-making planning related information, and a preset driving limiting condition, a reachable state set corresponding to the target object, where the preset hierarchical state machine includes: jump relations and hierarchical relations among the states corresponding to the target object;

a second obtaining module configured to obtain planning track information corresponding to each reachable state in the reachable state set;

the second determining module is configured to determine a target state corresponding to the target object from all reachable states based on planning track information corresponding to each reachable state, a level corresponding to each reachable state and evaluation factors, wherein the level corresponding to each reachable state is a level of the reachable state in a preset layered state machine;

and the control module is configured to control the preset hierarchical state machine to jump from the current state to the target state so as to enable a target object to run based on the planned track information corresponding to the target state.

Images