DE102017120366A1 - Method, apparatus, computer program and computer program product for motion planning of a system - Google Patents

Abstract

In a method for determining a movement plan, an initial state is determined. Depending on the initial state, a first invariant set is determined. A final state is determined. Depending on the final state, a second invariant set is determined. Depending on the initial state and the final state, a motion plan is determined. Prior to the execution of the movement plan, the movement plan is verified, wherein the verification is a review of a predetermined set of assumptions. A first interval is determined in which a return to the first invariant set is possible for the last time and safely using a collision-free trajectory. A second interval is determined in which a reaching of the second invariant set is first and certainly possible using a collision-free trajectory. If an assumption of the set of assumptions is violated when carrying out the movement plan, a trajectory to the first invariant set or to the second invariant set is determined depending on the first interval and the second interval.

Description

The invention relates to a method for motion planning of a system. The invention further relates to a device for motion planning of a system. The invention further relates to a computer program and a computer program product for motion planning of a system.

For many systems, especially vehicles, safety is the most important aspect to perform collision-free motion in dynamic environments.

The object underlying the invention is to contribute to a high degree of safety in the movement of a system.

The object is solved by the features of the independent claims. Advantageous embodiments are characterized in the subclaims.

The invention is characterized by a method for motion planning of a system. The invention is further characterized by a device for motion planning of a system, wherein the device is designed to perform the method or an optional embodiment of the method.

In the method, an initial state is determined which is representative of a state of the system at an initial time. Depending on the initial state, a first invariant set is determined which is representative of states in which the system is safe. A final state is determined which is representative of a state of the system at a target time. Depending on the final state, a second invariant set is determined which is representative of states in which the system is safe. Depending on the initial state and the final state, a motion plan is determined which is representative of a movement from the initial state to the final state. Prior to the execution of the movement plan, the movement plan is verified, wherein the verification is a review of a predetermined set of assumptions. A first interval is determined in which there is a point of no return, PNR, which is representative of a state in which a return to the first invariant set is possible for the last time and safely using a collision-free trajectory. A second interval is determined in which there is a point of guaranteed arrival, PGA, which is representative of a state in which reaching the second invariant set is first and certainly possible using a collision-free trajectory. If an assumption of the set of assumptions is violated when carrying out the movement plan, a trajectory to the first invariant set or to the second invariant set is determined depending on the first interval and the second interval.

By the above method, a very safe movement can be realized. The motion plan will be traversed by default by the system until a given assumption is violated. Subsequently, a very safe trajectory is determined, depending on the current position, position of the PGA and PNR, either back into the first invariant set or into the second invariant set to regain the security of the system. Thus, the method can be used arbitrarily, for example, when bypassing obstacles or lane changes or the like.

If no predefined assumption is violated, the determined movement plan is continued.

The system may be, for example, a power plant, an industrial robot, a controlled manufacturing process, in particular the system is a vehicle.

According to an optional embodiment, the movement plan during the verification is subdivided into safe and safety-critical phases depending on the first interval and the second interval.

According to a further optional embodiment, the movement plan is determined as a function of a cost function, wherein in the cost function a safest movement hypothesis corresponds to a movement hypothesis with the lowest cost.

Since safety-critical and safe phases of the movement plan can be determined on the basis of the intervals of PNR and PGA, these can also be quantified. This has the advantage of being able to assess the safety of multiple motion plan candidates. For this, the phases are associated with costs. Subsequently, the costs of the respective movement plans can then be compared and, depending on the modeling / system / scenario, the respectively optimal movement plan can be executed. The cost function can also be integrated directly into a motion planner so that it calculates safe trajectories according to the method.

According to a further optional embodiment, the trajectory is traveled autonomously to the first invariant set or to the second invariant set.

Especially with an autonomous driving safety is very important to a collision-free To perform movement in dynamic environments. Thus, it is particularly advantageous for autonomous driving to use a movement planning determined by the above method, for example for avoiding obstacles. By way of example, the obstacle may be a preceding vehicle to be overtaken, which is then autonomously overtaken by means of the method. Another example can be if you have to consider oncoming traffic in the overtaking process. If the verification shows that the oncoming traffic is far enough away, the movement plan for the overtaking process can be started. During the overtaking process, the situation may suddenly change, for example when the oncoming vehicle is driving much faster than expected. This violates an assumption, namely, for example, the assumption that the maximum permissible speed is maintained. In accordance with the method, an evasive maneuver is now determined on the basis of the invariant sets in order to recover the safety of the system or in this example of the vehicle. It is also important that in this scenario, the oncoming vehicle has made a mistake.

• - eine vorgegebene maximale absolute Beschleunigung wird von anderen Systemen nicht überschritten,
• - wenn eine vorgegebene maximale Geschwindigkeit erreicht wird, erfolgt keine weitere Beschleunigung von anderen Systemen in Fahrtrichtung,
• - wenn eine vorgegebene parametrisierte Geschwindigkeit erreicht wird, erfolgt maximal eine weitere vorgegebene Beschleunigung von anderen Systemen in Fahrtrichtung,
• - Fahrspuren dürfen von anderen Systemen nicht verlassen werden und ein Spurwechsel von anderen Systemen ist nur erlaubt auf Spuren in derselben Fahrtrichtung und nicht auf Spuren des Gegenverkehrs,
• - Rückwärtsfahren von anderen Systemen ist auf einer Spur nicht erlaubt.

According to another optional embodiment, the set of assumptions comprises at least one of the following assumptions:

• a predetermined maximum absolute acceleration is not exceeded by other systems,
• - if a given maximum speed is reached, there is no further acceleration from other systems in the direction of travel,
• if a predefined parameterized speed is reached, a maximum of one further predetermined acceleration takes place from other systems in the direction of travel,
• - lanes may not be left by other systems and a lane change from other systems is only allowed on lanes in the same direction of travel and not on lanes of oncoming traffic,
• - Reversing from other systems is not allowed on a lane.

These assumptions are advantageous if the system is a vehicle. For other systems, other assumptions may be important and assumptions may also be added or modified for vehicles.

According to a further aspect, the invention is characterized by a computer program, wherein the computer program is designed to carry out the method for motion planning or an optional embodiment of the method.

According to a further aspect, the invention is characterized by a computer program product comprising executable program code, wherein the program code, when executed by a data processing device, executes the motion planning method or an optional embodiment of the method.

In particular, the computer program product comprises a medium which can be read by the data processing device and on which the program code is stored.

• 1 ein Ablaufdiagram zur Bewegungsplanung,
• 2 eine Visualisierung von Zuständen beim Ausführen eines Bewegungsplan.

Embodiments of the invention are explained in more detail below with reference to the schematic drawings. Show it:

• 1 a flow diagram for motion planning,
• 2 a visualization of states when executing a motion plan.

The 1 shows a flow chart of a program for motion planning.

The program can be processed, for example, by a device. For this purpose, the device has in particular a computing unit, a program and data memory, as well as, for example, one or more communication interfaces. The program and data memory and / or the arithmetic unit and / or the communication interfaces can be formed in a structural unit and / or distributed over several structural units.

The program is stored in particular on the program and data memory of the device.

The device may also be referred to as a movement planning device.

The program will be in one step S1 started in which variables can be initialized if necessary.

In one step S3 an initial state is determined which is representative of a state of the system at an initial time.

In one step S5 Depending on the initial state, a first invariant set is determined, which is representative of states in which the system is safe.

In one step S7 a final state is determined that is representative of a state of the system at a target time.

In one step S9 Depending on the final state, a second invariant set is determined, which is representative of states in which the system is safe.

In one step S11 is determined depending on the initial state and the final state, a motion plan, which is representative of a movement from the initial state to the final state.

In one step S13 Before the execution of the movement plan, the movement plan is verified, whereby the verification results in a check of a predefined set of assumptions.

• - eine vorgegebene maximale absolute Beschleunigung wird von anderen Systemen nicht überschritten,
• - wenn eine vorgegebene maximale Geschwindigkeit erreicht wird, erfolgt keine weitere Beschleunigung von anderen Systemen in Fahrtrichtung,
• - wenn eine vorgegebene parametrisierte Geschwindigkeit erreicht wird, erfolgt maximal eine weitere vorgegebene Beschleunigung von anderen Systemen in Fahrtrichtung,
• - Fahrspuren dürfen von anderen Systemen nicht verlassen werden und ein Spurwechsel von anderen Systemen ist nur erlaubt auf Spuren in derselben Fahrtrichtung und nicht auf Spuren des Gegenverkehrs,
• - Rückwärtsfahren von anderen Systemen ist auf einer Spur nicht erlaubt.

The set of assumptions includes, for example, one or more of the following assumptions:

• a predetermined maximum absolute acceleration is not exceeded by other systems,
• - if a given maximum speed is reached, there is no further acceleration from other systems in the direction of travel,
• if a predefined parameterized speed is reached, a maximum of one further predetermined acceleration takes place from other systems in the direction of travel,
• - lanes may not be left by other systems and a lane change from other systems is only allowed on lanes in the same direction of travel and not on lanes of oncoming traffic,
• - Reversing from other systems is not allowed on a lane.

The assumptions can be checked, for example, by means of vehicle sensors and threshold comparisons.

In one step S15 a first interval is determined in which a point without recurrence, PNR , which is representative of a state in which a return to the first invariant set is possible for the last time and safely using a collision-free trajectory. Furthermore, in the step S15 determines a second interval in which a point of guaranteed arrival, PGA , which is representative of a state in which reaching the second invariant set is first and certainly possible using a collision-free trajectory. The step S15 can be performed several times, especially before and during the execution of the movement plan. For example, the intervals are roughly calculated before implementation and repeatedly checked during the implementation and calculated finer.

In one step S17 If, on execution of the movement plan, an assumption of the set of assumptions is violated, a trajectory to the first invariant set or to the second invariant set is determined as a function of the first interval and the second interval.

In one step S19 the program terminates and may again be in the step S1 to be started.

Optionally, the movement plan is subdivided into safe and safety-critical phases during verification, depending on the first interval and the second interval.

Optionally, the movement plan is determined as a function of a cost function, whereby in the cost function a safest movement hypothesis corresponds to a movement hypothesis with the lowest cost.

Optionally, the trajectory is traversed autonomously to the first invariant set or to the second invariant set.

In the following, an advantageous embodiment of the motion planning method will be described.

The following is an approach for verifying the security of systems in dynamic environments using invariant sets. An extension to an original definition of set invariance for time-dependent security areas is proposed. By using these sets, points of no return ( PNR ) and Points of Guaranteed Arrival (PGA). The properties of PNR and PGA allow an efficient safety assessment as described below. In time-critical situations where a previously verified motion plan suddenly becomes unsafe during execution, the above approach provides the following benefits:
additional security guarantees can be provided to find a feasible trajectory to safe states. PNR and PGA can be used to specify a useful function to assess the safety of multiple motion plans prior to their execution. Safety during execution can be increased even if unforeseen situations occur (assumptions are broken).

preliminaries

Set machbarer Zustände x und $U\subset {ℝ}^m$

ein Set zulässiger Steuereingaben u eines Systems f, welches der Differentialgleichung folgt $$\dot{x}\left(t\right)=f\left(x\left(t\right),u\left(t\right)\right).$$

Um eine Traj ektorie $u\left(t\right)\in U,t\in \left[\mathrm{0,}{t}_h\right]$

aus (1) zu beschreiben, wird im Folgenden die Notation u([0,t_h]) verwendet. Des Weiteren bezeichnet $X\left(t_h,x\left(0\right),u\left(\left[\mathrm{0,}{t}_h\right]\right)\right)\in X$

die Lösung von (1) zum Zeitpunkt t_h für x(0)=x₀ und u([0,t_h]).Describe below $X\subset {ℝ}^n$

Set of feasible states x and $U\subset {ℝ}^m$

a set permissible control inputs u of a system f, which follows the differential equation $$\dot{x}\left(t\right)=f\left(x\left(t\right).u\left(t\right)\right),$$

To a Traj ektorie $u\left(t\right)\in U.t\in \left[0t_H\right]$

from (1), the notation u ([0, t _h ]) is used below. Further referred to $X\left(t_H.x\left(0\right).u\left(\left[0t_H\right]\right)\right)\in X$

the solution of (1) at time t _h for x (0) = x ₀ and u ([0, t _h ]).

Definition 1 (safe state)

zum Zeitpunkt t, welches pfadverbunden ist, ist ein Zustand $x\left(t\right)$

sicher falls $x\left(t\right)\in F^t.$

Die Definition des Sets sicherer Zustände $F^t$

hängt vom System und seiner Umgebung ab.Assuming there is a set of safe states $F^t$

at time t, which is path-connected, is a state $x\left(t\right)$

certainly if $x\left(t\right)\in F^t,$

The definition of the set of safe states $F^t$

depends on the system and its environment.

Definition 2 (Safe Entry Trajectory)

ein sicherer Zustand ist. Definition 2 erlaubt die Sicherheit eines Regelungsgesetztes Φ zu beurteilen.An input trajectory u ([0, t]), 0 <t is called a safe input trajectory if $\forall r\in \left[0t\right]:X\left(\mathrm{\tau }.x\left(0\right).u\left(\left[0\mathrm{\tau }\right]\right)\right)$

a safe state. Definition 2 allows to assess the safety of a control law Φ.

Definition 3 (secure regulatory law)

A law of control Φ is certain when every induced input trajectory of the control law u (t) = _φsafe (x (t)) is a safe input trajectory. In the following, the notation u ([t ₁ , t ₂ ]) = φ _safe (x (t ₁ , t ₂ ])) is used to emphasize that a trajectory is generated. If one considers a safe law Φ _safe (x (t)), one can define an invariant set of safe states. The invariance set is time-dependent so that the invariance can be considered in dynamic environments. The time branching concept can be applied by introducing a separate time dimension τ next to the existing dimension t.

Definition 4 (Time-Dependent Safe Invariant Set)

gilt.A time-dependent secure invariant set (SIS) S ^t for a time t and the safe control law u (t) = Φ _SIS (x (t)) is defined as the set of states x at time t, such that $$x\left(t\right)\in S^t\Rightarrow \forall \mathrm{\tau }>\text{t}:X\left(\mathrm{\tau }.x\left(t\right).{\mathrm{\Phi }}_{SIS}\left(x\left(\left[t.\mathrm{\tau }\right]\right)\right)\right)\in S^t$$

applies.

) $$\forall \mathrm{\tau }\ge 0:S^t\subseteq F^t.$$

Proposition 1 (Relationship of S and $F$

) $$\forall \mathrm{\tau }\ge 0:S^t\subseteq F^t,$$

sind Zustände einer sicheren Trajektorie $x\left(t\right)\in F^t.$

Proof: x (t) ∈S ^t and $X\left(\mathrm{\tau }.x\left(t\right).{\mathrm{\Phi }}_{SIS}\left(x\left(\left[t.\mathrm{\tau }\right]\right)\right)\right)\in S^t.\mathrm{\tau }>\text{t}$

are states of a safe trajectory $x\left(t\right)\in F^t,$

Corollary 1 (Transition SIS)

Proposition 1 implies that S ^{t is} path-connected, which implies that (1) is capable of traversing the various S ^t over time t.

Theorem 1 (Safety for an Infinite Time Horizon)

Using Φ _SIS , the security of (1) can be guaranteed for an infinite time horizon even in dynamic environments.

Proof: Φ _SIS leaves (1) within S ^t at any time t independent of surrounding dynamic obstacles. In combination with subsequent Theorem 1 it follows that safety is maintained for an infinite time horizon.

als sicher betrachtet, jedoch nur für einen finiten Zeithorizont. Im Folgenden wird zur Übersichtlichkeit bei der Notation S auf die betrachtete Zeit t verzichtet.In contrast, the states become $x\left(t\right)\in \left(F^t/S^t\right):=\left\{x|x\in F^t\wedge x\notin S^t\right\}$

considered safe, but only for a finite time horizon. In the following, for the sake of clarity in the notation S, the time t considered is dispensed with.

Verification of movement plans

zu einem Endzustand $x\left(t_h\right)\in S_{post}^{t_h}$

übergehen muss. Sowohl $S_{pre}^0$

als auch $S_{post}^{t_h}$

sind SIS gemäß Definition 4. Es können Situationen auftreten, in denen $S_{pre}^0\cap S_{post}^{t_h}=\varnothing$

(in Kombination mit $\forall \mathrm{\tau }\in \left[\mathrm{0,}{t}_h\right]:S_{pre}^t\bigcap S_{post}^t=\varnothing$

wodurch die Möglichkeit eliminiert wird nur das dedizierte sichere Regelungsgesetz Φ_SIS zu verwenden. Daraus ergibt sich, dass nicht garantiert werden kann, ob die Trajektorie u([0,t_h]) für eine gegebene Aufgabe sicher ist gemäß Definition 2. Um die übergehende Trajektorie als kollisionsfrei zu verifizieren im Falle von umgebenden Hindernissen wird eine Erreichbarkeitsanalyse angewandt.In the following, the object is considered that the system (1) of an initial state $x\left(0\right)\in S_{\mathrm{<figure-callout\; id="0"\; label="p\; r\; e"\; filenames="DE102017120366A1\_0060.png"\; state="\{\{state\}\}">p\; </figure-callout>}re}^0$

to a final state $x\left(t_H\right)\in S_{pOst}^{t_H}$

has to go over. Either $S_{\mathrm{<figure-callout\; id="0"\; label="p\; r\; e"\; filenames="DE102017120366A1\_0060.png"\; state="\{\{state\}\}">p\; </figure-callout>}re}^0$

as well as $S_{pOst}^{t_H}$

are SIS as defined in 4. Situations may occur in which $S_{\mathrm{<figure-callout\; id="0"\; label="p\; r\; e"\; filenames="DE102017120366A1\_0060.png"\; state="\{\{state\}\}">p\; </figure-callout>}re}^0\cap S_{pOst}^{t_H}=\varnothing$

(in combination with $\forall \mathrm{\tau }\in \left[0t_H\right]:S_{pre}^t\bigcap S_{pOst}^t=\varnothing$

thereby eliminating the possibility of using only the dedicated safe control law Φ _SIS . As a result, it can not be guaranteed whether the trajectory u ([0, t _h ]) is safe for a given task according to Definition 2. In order to verify the passing trajectory as collision-free in the case of surrounding obstacles, a reachability analysis is used.

Definition 5 (reachable set)

aus (1) ist das Set an Zuständen, welche erreichbar sind an einem bestimmten Zeitpunkt r aus einem Set Initialzustände $X^0$

zum Zeitpunkt t₀ unter Berücksichtigung des Sets an Eingaben $U:R\left(r\right)=\left\{{\displaystyle {\int }_0^{r}f\left(x\left(t\right),u\left(t\right)\right)dt|x\left(0\right)\in X^0,\forall t:u\left(t\right)\in U}\right\}.$

Des Weiteren wird eine Zuordnung eingeführt des Zustandsraums auf das Set an Punkten im kartesischen Raum besetzt durch ein System (zusammen mit seinen Dimensionen).The achievable set $R\subseteq X$

from (1) is the set of states which can be reached at a certain point in time r from a set of initial states $X^{}$${}^0$

at time t ₀ taking into account the set of inputs $U:R\left(r\right)=\left\{{\displaystyle {\int }_0^{r}f\left(x\left(t\right).u\left(t\right)\right)dt|x\left(0\right)\in X^0.\forall t:u\left(t\right)\in U}\right\},$

Furthermore, an assignment of state space to the set of points in Cartesian space is introduced by a system (along with its dimensions).

Definition 6 (assignment in Cartesian space)

wobei $P\left({ℝ}^m\right)$

die Potenzmenge aus ℝ^m bezeichnet. Bei einem gegebenen Set an Zuständen $X$

ist die Zuordnung definiert als $map\left(X\right):=\left(map\left(x\right)|x\in X\right).$

The operator map (x) projects the state vector into Cartesian space according to world coordinates $$map\left(x\right):X\to P\left({ℝ}^m\right).m\le \mathrm{3,}$$

in which $P\left({ℝ}^m\right)$

denotes the power set of ℝ ^m . For a given set of states $X$

is the assignment defined as $map\left(X\right):=\left(map\left(x\right)|x\in X\right),$

Definition 7 (cast set)

das Set erreichbarer besetzter Punkte im kartesischen Raum: $$\left(t\right)=map\left(\left(t\right)\right).$$

Based on Definition 5 and Definition 6 describes the cast set $\left(t\right)$

the set of achievable occupied points in Cartesian space: $$\left(t\right)=map\left(\left(t\right)\right),$$

als kollisionsfrei verifiziert werden.Using cast sets, a motion plan can be designed taking into account the set of surrounding dynamic obstacles $B\subset \mathbb{N}$

be verified as collision-free.

Definition 8 (collision-free trajectory)

und der Besetzung des Ego-Systems ${ego}_{}\left(t\right)$

neben seiner geplanten Trajektorie u([0,t_h]) ist diese Trajektorie kollisionsfrei, falls $$\forall t\in \left[\mathrm{0,}{t}_h\right]:{ego}_{}\left(t\right)\cap {obs}_{}\left(t\right)=Ø.$$

Given the possible occupations of all surrounding obstacles ${Obs}_{}\left(t\right)={\text{U}}_{b\in B}{b}_{}\left(t\right)$

and the occupation of the ego system ${eGO}_{}\left(t\right)$

In addition to its planned trajectory u ([0, t _h ]), this trajectory is collision-free, if $$\forall t\in \left[0t_H\right]:{eGO}_{}\left(t\right)\cap {Obs}_{}\left(t\right)=O,$$

jedes Hindernisses $b\in B$

wird dabei basierend auf seinem Set an Initialzuständen $X_b^0$

und dem Set zulässiger Steuereingaben $U_b$

(vgl. Definition 5) ermittelt.The occupation $b_{}\left(t\right)$

every obstacle $b\in B$

is based on its set of initial states $X_b^{}$${}_0^{}$

and the set of valid control inputs $U_b$

(see definition 5).

und $U_b$

getroffen, da sich sonst ${obs}_{}\left(t\right)$

oftmals mit ${ego}_{}\left(t\right)$

überschneiden würde und das System daher nicht länger befähigt wäre, die Aufgabe sicher zu erfüllen.In order to limit the set of achievable states, restrictive assumptions are explained below. $X_b$

and $U_b$

met, because otherwise ${Obs}_{}\left(t\right)$

often with ${eGO}_{}\left(t\right)$

would overlap and therefore the system would no longer be able to perform the task safely.

Definition 9 (time-invariant assumptions)

Time-invariant assumptions A _∞ are assumptions which must be valid at all times, since they are necessary in order to be able to derive safe states of the system.

zu beschränken, werden ferner folgende Annahmen getroffen.For example, A _∞ includes physical constraints, such as limited acceleration, or general safety assumptions, such as dynamic obstacles that do not seek to force a collision with the ego system. The sets S _pre and S _post are based on A _∞ . To the cast sets ${Obs}_{}\left(t\right)$

Furthermore, the following assumptions are made.

Definition 10 (time-varying assumptions)

Time-varying assumptions A _t are assumptions that limit the movement of dynamic obstacles and could be violated at some point.

For example, A _t includes the assumption that the speed of road vehicles does not exceed an official speed limit up to a certain percentage. When dynamic obstacles violate time-varying assumptions, motion plan verification is no longer valid. Both sets, A _∞ and A _t , must be defined with respect to the system used and adapted to its environment and task. If the term acceptance is used below, it implicitly refers to A _t .

Improvement of safety by means of safe, invariant sets

To restore security to the system when time-varying assumptions have been violated, security-relevant points along u ([0, t _h ]) using S _pre and S _post proposed (see. 2 ).

Definition 11 (point of no return)

$$\forall t\in ]t_{PNR},t_h]:\cancel{\exists }u\left(\left[t,r\right]\right):X\left(r,x\left(t\right),u\left(\left[t,r\right]\right)\right)\in S_{pre}^r.$$

As a point of no return (PNR), the state x (t _PNR ), t _PNR ∈ [0, t _h ] is defined along u ([0, t _h ]), for which a return to S _pre last and safely possible Application of the collision-free trajectory u ([t, r]), t <r <t _h : $$\forall t\in \left[0t_{PNR}\right]:\exists u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pre}^r$$

$$\forall t\in ]t_{PNR}.t_H]:\cancel{\exists }u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pre}^r,$$

After a specific point along u ([0, t _h ]), the system is able to reach S _post without collision, regardless of the behavior of the dynamic obstacles.

Definition 12 (point of guaranteed arrival)

$$\forall t\in \left[t_{PNR},t_h\right]:\exists u\left(\left[t,r\right]\right):X\left(r,x\left(t\right),u\left(\left[t,r\right]\right)\right)\in S_{post}^r.$$

The point of guaranteed arrival ( PGA ) is the state x (t _PGA ), t _PGA ≤ t _{h of} the trajectory u ([0, t _h ]), from the point of safety using the collision-free trajectory u ([t, r]), t <r < t _{h is} guaranteed: $$\forall t\in [0t_{PGA}[:\cancel{\exists }u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pOst}^r$$

$$\forall t\in \left[t_{PNR}.t_H\right]:\exists u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pOst}^r,$$

Using Definition 11 and Definition 12, the safety-critical corridor is defined along u ([0, t _h ]).

Definition 13 (safety-critical corridor)

wird definiert als das Set an Zuständen x entlang u([0,t_h]) zwischen dem PNR und dem PGA: $SCP=\left\{x\left(t\right)|t_{PNR}<t<t_{PGA}\right\}.$

The Safety Critical Corridor (SCP) between $S_{\mathrm{<figure-callout\; id="0"\; label="p\; r\; e"\; filenames="DE102017120366A1\_0060.png"\; state="\{\{state\}\}">p\; </figure-callout>}re}^0\text{and}{S}_{pOst}^{t_H}$

is defined as the set of states x along u ([0, t _h ]) between the PNR and the PGA: $SCP=\left\{x\left(t\right)|t_{PNR}<t<t_{PGA}\right\},$

Determine the PNR and PGA

Instead of the exact position of points like the PNR and the PGA, only one time interval [ t , t ] of their possible positions x (t), t E [ t , t ] be determined. For the PNR, an upper bound can be determined by reachability analysis and a lower bound by sampling as demonstrated below.

Proposition 2 (approach from below)

A lower limit of a position of the PNR t _PNR is obtained by sampling trajectories in S _pre while examining the definition 11.

Proof: By definition, the set of sampled trajectories is a real subset of all feasible trajectories (1). Therefore, t _{PNR represents} an approximation from below.

Proposition 3 (approach from above)

unter Berücksichtigung von $X^0=\left\{x\left({\overline{t}}_{PNR}\right)\right\}.$

The upper limit t _PNR is determined by approximating sets that can be reached from above (see definition 5) by determining the minimum t _PNR so that $\left(t_H\right)S_{pre}^{t_H}=\varnothing$

considering $X^0=\left\{x\left({\overline{t}}_{PNR}\right)\right\},$

Proof: The proposition follows directly from the application of the approximated achievable sets of (1), which assures that the system is not capable of S _pre return from the determined upper limit.

The interval of the PGA can be determined analogously, the lower limit being an approach from above, which requires the reachability analysis, and the upper limit being an approximation from below, which requires sampling.

precalculation

A sufficient approximation of PNR - and of PGA Interval can be calculated in advance for a predefined task. This precalculated approach can be used as a starting point, which can be further refined below. In addition, both searches can be sped up by using a binary search strategy to determine the optimal bounds. Advantage of this strategy is that it can be applied at any time to refine the limits in term.

safety assessment

Within [ t , t ], no statements can be made about the safety of the system.

Significance of movement safety

Daher ist der Korridor SCP, welcher basierend auf dem Initialset an Annahmen errechnet wurde, möglichen Kollisionen ausgesetzt (vgl. 1).It is advantageous to make assumptions to verify the motion plan u ([0, t _h ]). A violation of the assumptions results in larger achievable sets of obstacles ${Obs}_{},$

Therefore, the corridor SCP, which has been calculated based on the initial set of assumptions, is exposed to possible collisions (cf. 1 ).

In order to be able to make statements about the safety of the trajectories, in particular if assumptions were violated, the points PNR and PGA are used.

Theorem 2 (Safe and safety-critical stages)

1. 1) t ∈ [0,t_PNR]: eine machbare und kollisionsfreie Trajektorie in einen sicheren Zustand $x\in S_{pre}$
   
   ist garantiert bis hin zu dem PNR: $\forall t\in \left[\mathrm{0,}{\underset{\_}{t}}_{PNR}\right][:\exists u\left(\left[t,r\right]\right):X\left(r,x\left(t\right),u\left(\left[t,r\right]\right)\right)\in S_{post}^r.$
   
2. 2) t ∈ [t _PNR, t_PGA]: es muss keine kollisionsfreie Traj ektorie innerhalb des SCP existieren.
3. 3) t ∈ [t _PGA,t_h]: eine machbare und kollisionsfreie Traj ektorie in einen sicheren Zustand $x\in S_{post}$
   
   ist garantiert ab dem PGA: $\forall t\in \left[\mathrm{0,}{\overline{t}}_{PGA},t_h\right]:\exists u\left(\left[t,r\right]\right):X\left(r,x\left(t\right),u\left(\left[t,r\right]\right)\right)\in S_{post}^r.$
   

The movement plan u ([0, t _h ]) can be subdivided into safe and safety-critical stages using the PNR and PGA:

1. 1) t ∈ [0, t _PNR ]: a feasible and collision-free trajectory in a safe state $x\in S_{pre}$
   
   is guaranteed right down to the PNR: $\forall t\in \left[0{\underset{\_}{t}}_{PNR}\right][:\exists u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pOst}^r,$
   
2. 2) t ∈ [ t _PNR , t _PGA ]: there must be no collision-free trajectory within the SCP.
3. 3) t ∈ [ t _PGA , t _h ]: a feasible and collision-free traj ectorie in a safe condition $x\in S_{pOst}$
   
   is guaranteed from the PGA: $\forall t\in \left[0{\overline{t}}_{PGA}.t_H\right]:\exists u\left(\left[t.r\right]\right):X\left(r.x\left(t\right).u\left(\left[t.r\right]\right)\right)\in S_{pOst}^r,$
   

Proof: theorem 2 follows directly from definition 11 to definition 13 , Once the system is in S _pre or S _post it can change into a proper safe control law Φ _SIS and remain safe for an infinite time horizon.

The motion planner can use safety critical stages to evaluate trajectories.

Proposition 4 (security costs)

The certainty of k different motion plans u _i ([0, t _h ]), i ≤ k can be evaluated by using the corridors SCP _{u i} Costs are assigned and these costs are compared with a cost function c (u ([0, t _h ])): SCP _u → ℝ.

Proposition 4 is by definition 13 derived and allows a characterization and comparison of the corridors SCP _{u i} different movement plans u _i ([0, t _h ]). The costs c have to be modeled depending on specific tasks and the system used. For example, the cost c may correspond to a time period when the system is located in the corridor. The safest movement hypothesis u * ([0, t _h ]) corresponds to the one with the lowest cost c, eg $$u*\left(\left[0t_H\right]\right)=\text{bad}\underset{i\le k}{\text{min}}\text{c}\left(u_i\left(\left[0t_H\right]\right)\right),$$

In the prior art, motion planners are incapable of considering the SCP, which can result in trajectories with large safety-critical corridors. By integrating the costs of the corridors as separate costs in the optimization, the motion planner can determine the safest trajectory u * ([0, t _h ]) with respect to the corridors. Thus, the motion planner may be capable of determining a trajectory with a zero-size corridor.

Proposition 5 (zero corridor)

SCP = ∅ of a motion plan u ([0, t _h ]) guarantees that the system is always capable of S _pre and S _post collision-free, regardless of time-varying assumptions.

Proposition 5 implies that the security of the system can be guaranteed for an infinite time horizon. This is due to the fact that, from each state x (t) along the trajectory u ([0, t _h ]), a feasible and collision-free trajectory can be determined that is either in S _pre or S _post ends.

Claims (8)

A motion planning method of a system, comprising: - determining an initial state representative of a state of the system at an initial time, - determining, based on the initial state, a first invariant set representative of states in which the system is safe , - a final state is determined, which is representative of a state of the system at a target time, - depending on the final state, a second invariant set is determined which is representative of states in which the system is safe, - depending on the initial state and Finally, a motion plan is determined, which is representative of a movement from the initial state to the final state, - is verified prior to execution of the movement plan of the movement plan, wherein the verification is carried out a check of a predetermined set of assumptions, - a first interval is determined in which is a point of no return, PNR t, that representative of a state in which a return to the first invariant set is possible for the last time and safely using a collision-free trajectory, - determining a second interval in which a guaranteed arrival point, PGA, is representative of one State in which a reaching of the second invariant set is first and certainly possible using a collision-free trajectory, - if an assumption of the set of assumptions is violated when carrying out the movement plan, depending on the first interval and the second interval, a trajectory to the first invariant set or to the second invariant set is determined. Method according to Claim 1 in which the movement plan during the verification is subdivided into safe and safety-critical phases depending on the first interval and the second interval. Method according to Claim 2 in which the movement plan is determined as a function of a cost function, wherein in the cost function a safest movement hypothesis corresponds to a movement hypothesis with the lowest cost. Method according to one of the preceding claims, in which the trajectory is traveled autonomously to the first invariant set or to the second invariant set. Method according to one of Claims 1 to 4 in which the predetermined set of assumptions comprises at least one of the following assumptions: - a given maximum absolute acceleration is not exceeded by other systems, - if a given maximum speed is reached, no further acceleration from other systems takes place in the direction of travel, - if a given maximum parameterized speed is reached, there is at most one further predetermined acceleration from other systems in the direction of travel, - Lanes may not be left by other systems and a lane change from other systems is only allowed on tracks in the same direction and not on tracks of oncoming traffic, - reversing from other systems are not allowed on a track. Motion planning device, wherein the device is adapted to a method according to one of Claims 1 to 5 perform. Computer program for motion planning, wherein the computer program is formed by a method according to one of Claims 1 to 5 when executed on a data processing device. Computer program product comprising executable program code, wherein the program code when executed by a data processing device, the method according to one of Claims 1 to 5 performs.

Images