# US 11,999,376 B2 · Systems, methods, and computer-readable media for spatio-temporal motion planning

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

- Number: US 11,999,376 B2
- Document ID: US11999376B2
- Status: Granted
- Kind: B2
- Application number: 17/590,707
- Filed: 2022-02-01
- Published: 2023-08-03
- Granted: 2024-06-04
- Earliest claimed: 2022-02-01
- Assignees: Huawei Technologies Co., Ltd.
- Applicants: Weize Zhang; Peyman Yadmellat
- Inventors: Weize Zhang; Peyman Yadmellat
- Web page: https://patentgrep.com/patent/US11999376B2

## CPC

- B60W 60/001: {Planning or execution of driving tasks}
- B60W 60/0011: {involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles}
- B60W 40/107: Longitudinal acceleration
- B60W 40/109: Lateral acceleration
- B60W 2554/4041: Position
- 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

## Abstract

Systems, methods and computer-readable media for spatio-temporal motion planning, including: receiving data defining a drivable area within a spatio-temporal space; generating a cost function to be minimized that includes one or more upper-bound cost terms, each upper-bound cost term approximating the upper-bound of a cost term for the trajectory for a spatial frame of the spatio-temporal space, each upper-bound cost term being expressed as one or more longitudinal-temporal cost terms and one or more lateral-temporal cost terms for the trajectory; and further cost terms for the trajectory for the spatio-temporal frames; computing, based on the constraints and cost function, a planned trajectory through the drivable area, the planned trajectory directory being optimized with respect to both spatial frame and the spatio-temporal frames. Optimizing the planned trajectory in the spatial frame in addition to the spatio-temporal frames can enable planning objectives for the spatial frame to be optimized, for example maintaining closeness to a reference path and minimizing lateral acceleration and directional oscillation.

## Claims

### Claim 1

A computer implemented method for motion planning for an autonomous vehicle, comprising:
receiving data defining a drivable area within a spatio-temporal space that includes first and second spatial dimensions and a time dimension, the first and second spatial dimensions defining a spatial frame, the first spatial dimension and time dimension defining a first spatio-temporal frame, and the second spatial dimension and time dimension defining a second spatio-temporal frame;
generating a set of constraints for a trajectory within the drivable area;
generating a cost function to be minimized that includes one or more upper-bound cost terms, each upper-bound cost term approximating the upper-bound of a cost term for the trajectory for the spatial frame, each upper-bound cost term being expressed as one or more longitudinal-temporal cost terms for the trajectory for the first spatio-temporal frame and one of more lateral-temporal cost terms for the second spatio-temporal frame;
computing, based on the set of constraints and the cost function, a planned trajectory through the drivable area, the planned trajectory directory being optimized with respect to each of the spatial frame, the first spatio-temporal frame and the second spatio-temporal frame; and
outputting the planned trajectory for controlling an operation of the autonomous vehicle.

### Claim 2 (depends on 1)

The method of claim 1, wherein the cost function further includes further cost terms for the trajectory for the first spatio-temporal frame and the second spatio-temporal frame.

### Claim 3 (depends on 1)

The method of claim 1, wherein the set of constraints and the cost function collectively provide an optimization problem and computing the planned trajectory comprises using an optimization problem solver to determine an optimized solution based on the optimization problem.

### Claim 4 (depends on 1)

The method of claim 1, wherein the first spatial dimension corresponds to a longitudinal direction of travel in the drivable area and second spatial dimension corresponds to a direction that is perpendicular to the longitudinal direction.

### Claim 5 (depends on 4)

The method of claim 4, wherein the cost term for the trajectory for the spatial frame includes one or more of the following cost terms for the trajectory for the spatial frame: a lateral difference of the trajectory projected onto a first plane relative to a reference trajectory projected onto the first plane; a difference between the lateral derivative of the trajectory with respect to the longitudinal coordinate (s) relative to that of a reference trajectory; or changes in the lateral derivatives of the trajectory with respect to the longitudinal coordinate (s) over time.

### Claim 6 (depends on 1)

The method of claim 1, wherein the drivable area within the spatio-temporal space is defined using Frenet SLT coordinates, where the S coordinate corresponds to the first spatial dimension and denotes a longitudinal distance along a tangent vector of a driving zone, the L coordinate corresponds to the second spatial dimension and denotes a lateral distance to a normal vector of the driving zone, and the T coordinate corresponds to the time dimension and denotes time, the spatial frame is the SL frame, the first spatio-temporal frame is the ST frame and the second spatio-temporal frame is the LT frame.

### Claim 7 (depends on 6)

The method of claim 6, wherein the cost term for the trajectory for the spatial frame includes one or more of the following SL frame cost terms for the trajectory projected onto the SL frame:
∫
0
S
T
horizon
[
l
⁡
(
s
)
-
l
r
(
s
)
]
2
⁢
ds
(
a
)
∫
0
S
T
horizon
[
d
d
⁢
s
⁢
l
⁡
(
s
)
-
d
d
⁢
s
⁢
l
r
(
s
)
]
2
⁢
ds
,
and
(
b
)
∫
0
S
T
horizon
[
d
d
⁢
t
⁢
d
d
⁢
s
⁢
l
⁡
(
s
)
]
2
⁢
d
⁢
s
(
c
)
where: l(s) is a projection of the trajectory in a SL plane and lr(s) is a projection of a reference trajectory in the SL plane, Thorizon is a horizon of a time window of interest, and SThorizon is an arc length along the reference trajectory in the time window of interest.

### Claim 8 (depends on 7)

The method of claim 7, wherein:
the upper-bound of the SL frame cost term (a) is approximated by:
∑
i
=
0
n
-
1
(
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
+
1
k
1
⁢
d
⁢
l
d
⁢
t
⁢
(
t
i
)
⁢
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
d
⁢
s
d
⁢
t
⁢
(
t
i
)
)
⁢
∫
t
i
t
i
+
1
[
l
⁡
(
t
)
-
l
r
(
t
)
]
2
⁢
dt
+
∑
i
=
0
n
-
1
(
k
1
⁢
d
⁢
l
d
⁢
t
⁢
(
t
i
)
⁢
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
d
⁢
s
d
⁢
t
⁢
(
t
i
)
)
⁢
∫
t
i
t
i
+
1
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
d
⁢
t
the upper-bound of the SL frame cost term (b) is approximated by:
∑
i
=
0
n
-
1
(
1
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
+
η
⁡
(
t
i
)
k
2
)
⁢
∫
t
i
t
i
+
1
[
d
⁢
l
⁡
(
t
)
d
⁢
t
-
d
⁢
l
r
(
t
)
d
⁢
t
]
2
⁢
dt
+
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
k
2
⁢
η
⁡
(
t
i
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
d
⁢
t
or
the upper-bound of the SL frame cost term (c) is approximated by:
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
2
-
k
3
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
dl
⁡
(
t
)
dt
]
2
⁢
dt
+
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
1
-
1
k
3
⁢
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
dl
2
(
t
)
dt
2
]
2
⁢
dt
where: k1, k2 and k3 are predefined positive numbers, n is a predefined positive integer, s(t) is a projection of the trajectory in a ST plane, l(t) is a projection of the trajectory in a LT planes and sr(t) and lr are projections of the reference trajectory in the ST plane and the LT plane, respectively, and
η
⁡
(
t
)
=
1
d
⁢
s
r
d
⁢
t
⁢
(
t
)
⁢
(
-
d
2
⁢
s
d
⁢
t
2
⁢
(
t
)
⁢
d
⁢
l
d
⁢
t
⁢
(
t
)
(
d
⁢
s
d
⁢
t
⁢
(
t
)
)
2
+
d
2
⁢
l
d
⁢
t
2
⁢
(
t
)
d
⁢
s
d
⁢
t
⁢
(
t
)
)
.

### Claim 9 (depends on 2)

The method of claim 2, wherein the further cost terms comprise one or more of:
∫
0
T
horizon
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
d
d
⁢
t
⁢
s
⁡
(
t
)
-
d
d
⁢
t
⁢
s
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
l
⁡
(
t
)
-
l
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
d
d
⁢
t
⁢
l
⁡
(
t
)
-
d
d
⁢
t
⁢
l
r
(
t
)
]
2
⁢
d
⁢
t
where s(t) is a projection of the trajectory in a ST plane, l(t) is a projection of the trajectory in a LT planes, and sr(t) and lr(t) are projections of the reference trajectory in the ST plane and the LT plane, respectively.

### Claim 10 (depends on 1)

The method of claim 1, wherein generating the set of constraints for the trajectory within the drivable area comprises generating approximating constraints that approximate constraints for the trajectory for the spatial frame, the approximated constraints being expressed as constraints for the trajectory for the first spatio-temporal frame and the second spatio-temporal frame.

### Claim 11 (depends on 1)

The method of claim 1, comprising controlling at least one of a steering operation and a throttle operation of the autonomous vehicle to effect the planned trajectory.

### Claim 12

A system for motion planning for an autonomous vehicle, the system comprising one or more processors and a memory storing executable instructions that, when executed by the one or more processors configure the system to perform a method comprising:
receiving data defining a drivable area within a spatio-temporal space that includes first and second spatial dimensions and a time dimension, the first and second spatial dimensions defining a spatial frame, the first spatial dimension and time dimension defining a first spatio-temporal frame, and the second spatial dimension and time dimension defining a second spatio-temporal frame;
generating a set of constraints for a trajectory within the drivable area;
generating a cost function to be minimized that includes one or more upper-bound cost terms, each upper-bound cost term approximating the upper-bound of a cost term for the trajectory for the spatial frame, each upper-bound cost term being expressed as one or more longitudinal-temporal cost terms for the trajectory for the first spatio-temporal frame and one or more lateral-temporal cost terms for the second spatio-temporal frame;
computing, based on the set of constraints and the cost function, a planned trajectory through the drivable area, the planned trajectory directory being optimized with respect to each of the spatial frame, the first spatio-temporal frame and the second spatio-temporal frame; and
outputting the planned trajectory for controlling an operation of the autonomous vehicle.

### Claim 13 (depends on 12)

The system of claim 12, wherein the cost function further includes further cost terms for the trajectory for the first spatio-temporal frame and the second spatio-temporal frame.

### Claim 14 (depends on 12)

The system of claim 12, wherein the set of constraints and the cost function collectively provide an optimization problem and computing the planned trajectory comprises using an optimization problem solver to determine an optimized solution based on the optimization problem.

### Claim 15 (depends on 12)

The system of claim 12, wherein the first spatial dimension corresponds to a longitudinal direction of travel in the drivable area and second spatial dimension corresponds to a direction that is perpendicular to the longitudinal direction.

### Claim 16 (depends on 12)

The system of claim 12, wherein the cost term for the trajectory for the spatial frame includes one or more of the following cost terms for the trajectory for the spatial frame: a lateral difference of the trajectory projected onto the first plane relative to a reference trajectory projected onto a first plane; a difference between the lateral derivative of the trajectory with respect to the longitudinal coordinate (s) relative to that of a reference trajectory; or changes in the lateral derivatives of the trajectory with respect to the longitudinal coordinate (s) over time.

### Claim 17 (depends on 12)

The system of claim 12, wherein the drivable area within the spatio-temporal space is defined using Frenet SLT coordinates, where the S coordinate corresponds to the first spatial dimension and denotes a longitudinal distance along a tangent vector of a driving zone, the L coordinate corresponds to the second spatial dimension and denotes a lateral distance to a normal vector of the driving zone, and the T coordinate corresponds to the time dimension and denotes time, the spatial frame is the SL frame, the first spatio-temporal frame is the ST frame and the second spatio-temporal frame is the LT frame.

### Claim 18 (depends on 17)

The system of claim 17, wherein the cost term includes one or more of the following SL frame cost terms of the trajectory projected onto the SL frame:
∫
0
S
T
horizon
[
l
⁡
(
s
)
-
l
r
(
s
)
]
2
⁢
ds
(
a
)
∫
0
S
T
horizon
[
d
d
⁢
s
⁢
l
⁡
(
s
)
-
d
d
⁢
s
⁢
l
r
(
s
)
]
2
⁢
ds
,
and
(
b
)
∫
0
S
T
horizon
[
d
d
⁢
t
⁢
d
d
⁢
s
⁢
l
⁡
(
s
)
]
2
⁢
d
⁢
s
(
c
)
where: l(s) is a projection of the trajectory in a SL plane and lr(s) is a projection of a reference trajectory in the SL plane, Thorizon is a horizon of a time window of interest, and SThorizon is an arc length along the reference trajectory in the time window of interest.

### Claim 19 (depends on 18)

The system of claim 18 wherein:
the SL frame cost term (a) is approximated by:
∑
i
=
0
n
-
1
(
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
+
1
k
1
⁢
d
⁢
l
d
⁢
t
⁢
(
t
i
)
⁢
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
d
⁢
s
d
⁢
t
⁢
(
t
i
)
)
⁢
∫
t
i
t
i
+
1
[
l
⁡
(
t
)
-
l
r
(
t
)
]
2
⁢
dt
+
∑
i
=
0
n
-
1
(
k
1
⁢
d
⁢
l
d
⁢
t
⁢
(
t
i
)
⁢
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
d
⁢
s
d
⁢
t
⁢
(
t
i
)
)
⁢
∫
t
i
t
i
+
1
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
d
⁢
t
the SL frame cost term (b) is approximated by:
∑
i
=
0
n
-
1
(
1
d
⁢
s
r
d
⁢
t
⁢
(
t
i
)
+
η
⁡
(
t
i
)
k
2
)
⁢
∫
t
i
t
i
+
1
[
d
⁢
l
⁡
(
t
)
d
⁢
t
-
d
⁢
l
r
(
t
)
d
⁢
t
]
2
⁢
dt
+
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
k
2
⁢
η
⁡
(
t
i
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
d
⁢
t
or
the SL frame cost term (c) is approximated by:
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
2
-
k
3
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
dl
⁡
(
t
)
dt
]
2
⁢
dt
+
∑
i
=
0
n
-
1
❘
"\[LeftBracketingBar]"
1
-
1
k
3
⁢
(
d
2
⁢
s
dt
2
⁢
(
t
i
)
ds
dt
⁢
(
t
i
)
)
❘
"\[RightBracketingBar]"
⁢
∫
t
i
t
i
+
1
[
dl
2
(
t
)
dt
2
]
2
⁢
dt
where: k1, k2 and k3 are predefined positive numbers, n is a predefined positive integer, s(t) is a projection of the trajectory in a ST plane, l(t) is a projection of the trajectory in and a LT planes and sr(t) and lr(t) are projections of the reference trajectory in the ST plane and the LT plane, respectively, and
η
⁡
(
t
)
=
1
d
⁢
s
r
d
⁢
t
⁢
(
t
)
⁢
(
-
d
2
⁢
s
d
⁢
t
2
⁢
(
t
)
⁢
d
⁢
l
d
⁢
t
⁢
(
t
)
(
d
⁢
s
d
⁢
t
⁢
(
t
)
)
2
+
d
2
⁢
l
d
⁢
t
2
⁢
(
t
)
d
⁢
s
d
⁢
t
⁢
(
t
)
)
.

### Claim 20 (depends on 12)

The system of claim 12 wherein the further cost terms comprise one or more of:
∫
0
T
horizon
[
s
⁡
(
t
)
-
s
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
d
d
⁢
t
⁢
s
⁡
(
t
)
-
d
d
⁢
t
⁢
s
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
l
⁡
(
t
)
-
l
r
(
t
)
]
2
⁢
dt
∫
0
T
horizon
[
d
d
⁢
t
⁢
l
⁡
(
t
)
-
d
d
⁢
t
⁢
l
r
(
t
)
]
2
⁢
d
⁢
t
where s(t) is a projection of the trajectory in a ST plane, l(t) is a projection of the trajectory in a LT plane and sr(t) and lr(t) are projections of the reference trajectory in the ST plane and the LT plane, respectively.

### Claim 21

A non-transitory computer-readable medium storing instructions which, when executed by execution by a processing system, cause the processing system to perform a method comprising:
receiving data defining a drivable area within a spatio-temporal space that includes first and second spatial dimensions and a time dimension, the first and second spatial dimensions defining a spatial frame, the first spatial dimension and time dimension defining a first spatio-temporal frame, and the second spatial dimension and time dimension defining a second spatio-temporal frame;
generating a set of constraints for a trajectory within the drivable area;
generating a cost function to be minimized that includes one or more upper-bound cost terms, each upper-bound cost term approximating the upper-bound of a cost term for the trajectory for the spatial frame, each upper-bound cost term being expressed as one or more longitudinal-temporal cost terms for the trajectory for the first spatio-temporal frame and one or more lateral-temporal cost terms for the second spatio-temporal frame;
computing, based on the set of constraints and the cost function, a planned trajectory through the drivable area, the planned trajectory directory being optimized with respect to each of the spatial frame, the first spatio-temporal frame and the second spatio-temporal frame; and
outputting the planned trajectory for controlling an operation of the autonomous vehicle.