# US 2021/0078596 A1 · Safe trajectory tracking in uncertain environments

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2021/0078596 A1
- Document ID: US20210078596A1
- Status: Published application
- Kind: A1
- Application number: 17/017,870
- Filed: 2020-09-11
- Published: 2021-03-18
- Earliest claimed: 2019-09-13
- Assignees: Zenuity AB
- Applicants: Zenuity AB
- Inventors: Ivo BATKOVIC; Mohammad ALI; Mario ZANON; Paolo FALCONE
- Web page: https://patentgrep.com/patent/US20210078596A1

## CPC

- B60W 60/0015: {specially adapted for safety}
- B60W 60/0011: {involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles}
- G01C 21/3453: {Special cost functions, i.e. other than distance or default speed limit of road segments}
- G05D 1/0214: {in accordance with safety or protection criteria, e.g. avoiding hazardous areas (monitoring the location of vehicles within a certain area, e.g. forbidden or allowed areas, in traffic control systems for road vehicles G08G1/13)}
- G05D 1/0217: {in accordance with energy consumption, time reduction or distance reduction criteria}
- G05D 1/0223: {involving speed control of the vehicle (vehicle fittings for automatically controlling, i.e. preventing speed from exceeding an arbitrarily established velocity or maintaining speed at a particular velocity, as selected by the vehicle operator B60K31/00)}
- B60W 2720/103: Speed profile
- B60W: CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S)
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES

## Abstract

The present disclosure relates to a method for trajectory planning for a vehicle. The method includes obtaining a reference trajectory over a finite time horizon, where the reference trajectory includes a speed reference over time for the finite time horizon. Further, the method includes determining a back-up stop trajectory within the finite time horizon. The back-up stop trajectory has a starting state and terminating in a final state, where the final state is defined as a safe state. The method further includes forming a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states includes a terminal state that corresponds to the starting state for the back-up stop trajectory. Moreover, the method includes generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique.

## Claims

### Claim 1

A method for trajectory planning for a vehicle, the method comprising:
obtaining a reference trajectory over a finite time horizon, the reference trajectory comprising a speed reference over time for the finite time horizon;
determining a back-up stop trajectory within the finite time horizon, the back-up stop trajectory having a starting state and terminating in a final state, the final state being defined as a safe state;
forming a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states comprises a terminal state that corresponds to the starting state for the back-up stop trajectory;
generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, the nominal trajectory being dependent on the obtained reference trajectory and a terminal constraint, wherein the terminal constraint defines that the nominal trajectory comprises the terminal state; and
wherein the constraint controlled technique comprises a cost minimizing control strategy, and wherein the back-up stop trajectory from the starting state to the final state is associated with zero cost.

### Claim 2 (depends on 1)

The method according to claim 1, wherein the back-up stop trajectory is associated with zero cost for the generated nominal trajectory as long as the corresponding terminal state of the terminal set of states is comprised in the nominal trajectory.

### Claim 3 (depends on 1)

The method according to claim 1, wherein the nominal trajectory ends with a terminal state from the terminal set of states.

### Claim 4 (depends on 1)

The method according to claim 1, wherein the constraint controlled technique is based on a model predictive control, MPC, framework.

### Claim 5 (depends on 1)

The method according to claim 1, wherein the finite time horizon comprises a nominal time horizon defined as a first portion of the finite time horizon, and a back-up stop time horizon defined as a second portion of the finite time horizon, the first portion preceding the second portion; and
wherein the nominal trajectory is comprised within the nominal time horizon and the back-up stop trajectory is comprised within the back-up stop time horizon.

### Claim 6 (depends on 5)

The method according to claim 5, wherein the first portion is a first predefined time period of the finite time horizon and wherein the second portion is a second predefined time period of the finite time horizon, the first predefined time period and the second predefined time period being equal to a total time of the finite time horizon.

### Claim 7 (depends on 1)

The method according to claim 1, wherein the final state is a state in which a speed of the vehicle is zero.

### Claim 8 (depends on 1)

The method according to claim 1, wherein the at least one predefined constraint comprises at least one of a road geometry of a surrounding environment of the vehicle, a maximum deceleration capacity of the vehicle, and weather data.

### Claim 9 (depends on 1)

The method according to claim 1, further comprising:
for a first time sample, sending a signal in order to control a speed of the vehicle based on the generated nominal trajectory; and
shifting the finite time horizon forward in time by one time sample; and
repeating the method according to any one of the preceding claims where the finite time horizon is replaced by the shifted finite time horizon.

### Claim 10 (depends on 1)

The method according to claim 1, further comprising:
obtaining a predicted trajectory of at least one dynamic object located in a surrounding environment of the vehicle, wherein the predicted trajectory is based on sensor data obtained by at least one sensor and a predefined model; and
wherein the terminal set of states is formed further based on each predicted trajectory.

### Claim 11

A computer-readable storage medium storing one or more programs configured to be executed by one or more processors of a vehicle control system, the one or more programs comprising instructions for performing the method for trajectory planning for a vehicle, the method comprising:
obtaining a reference trajectory over a finite time horizon, the reference trajectory comprising a speed reference over time for the finite time horizon;
determining a back-up stop trajectory within the finite time horizon, the back-up stop trajectory having a starting state and terminating in a final state, the final state being defined as a safe state;
forming a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states comprises a terminal state that corresponds to the starting state for the back-up stop trajectory;
generating a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, the nominal trajectory being dependent on the obtained reference trajectory and a terminal constraint, wherein the terminal constraint defines that the nominal trajectory comprises the terminal state; and
wherein the constraint controlled technique comprises a cost minimizing control strategy, and wherein the back-up stop trajectory from the starting state to the final state is associated with zero cost.

### Claim 12

A control device for trajectory planning for a vehicle, the control device comprising control circuitry configured to:
obtain a reference trajectory over a finite time horizon, the reference trajectory comprising a speed reference over time for the finite time horizon;
determine a back-up stop trajectory within the finite time horizon, the back-up stop trajectory having a starting state and a final state, the final state being defined as a safe state;
form a terminal set of states within the finite time horizon based on at least one predefined constraint, wherein the terminal set of states comprises a terminal state that corresponds to the starting state for the back-up stop trajectory;
generate a nominal trajectory for at least a portion of the finite time horizon based on a constraint controlled technique, the nominal trajectory being dependent on the obtained reference trajectory and a terminal constraint, wherein the terminal constraint defines that the nominal trajectory comprises the terminal state; and
wherein the constraint controlled technique comprises a cost minimizing control strategy, and wherein the back-up stop trajectory from the starting state to the final state is associated with zero cost.

### Claim 13 (depends on 12)

A vehicle comprising a control device according to claim 12.
