Trajectory planning
A method and apparatus for determining a trajectory for a vehicle can include: identifying a starting position (p 0 ) and desired terminal position (P) for the vehicle; linearly approximating dynamics of the vehicle; and using the starting position (p 0 ), terminal position (P), and linear approximation, determining the trajectory; wherein the linear approximation can be constrained by a requirement (e.g., specification) that the vehicle may not travel in a region; a center point of the region is at a distance from a predetermined point on the vehicle substantially equal to a minimum turn radius of the vehicle, in a direction substantially perpendicular to a velocity of the vehicle; and a distance from the center point of the region to a point on a perimeter of the region is greater than or substantially equal to the minimum turn radius of the vehicle.
1. A method for controlling a land vehicle having a curvature limit according to a trajectory, the method comprising:
identifying a starting position (p 0 ) for the vehicle;
identifying a desired terminal position (P) for the vehicle;
defining a linear approximation of dynamics of the vehicle with a state vector (x) that indicates land vehicle velocity with linearized variables that lack non-linear heading information;
determining, based on the starting position (p 0 ), the desired terminal position (P), and the linear approximation, by the one or more processors, the trajectory for the land vehicle that optimizes a linear time invariant system having a form {dot over (x)}=Ax+Bu against a cost function and with a first constraint, and without a second constraint, applied to the state vector (x) to determine an acceleration input (u) to be applied by the land vehicle to accomplish a portion of the trajectory;
determining, based on the starting position (p 0 ), the desired terminal position (P), and the linear approximation, by the one or more processors, the trajectory for the land vehicle that optimizes a linear time invariant, system having a form {dot over (x)}=Ax+Bu against a cost function without the first constraint and with the second constraint applied to the state vector (x) to determine an acceleration input (u) to be applied by the land vehicle to accomplish another portion of the trajectory; and
controlling the land vehicle to follow the trajectory;
wherein the second constraint provides that:
the vehicle may not travel in a region;
a centre point of the region is at a distance from a predetermined point on the vehicle substantially equal to a minimum turn radius of the vehicle, in a direction substantially perpendicular to a velocity of the vehicle; and
a distance from the centre point of the region to a point on a perimeter of the region is greater than or substantially equal to the minimum turn radius of the vehicle;
wherein the first constraint includes at least one of:
a constraint on magnitude of acceleration of the vehicle;
a constraint on magnitude of velocity of the vehicle; and
a constraint that acceleration is larger when acting in a direction that is substantially perpendicular to velocity of the land vehicle than when acting in a direction that is substantially parallel to velocity of the land vehicle.
2. A method according to claim 1 , wherein the region is a polygon.
3. A method according to claim 2 , wherein the point on the perimeter of the region is a corner of the polygon.
4. A method according to claim 1 , wherein the linear approximation is further constrained by a specification that:
the vehicle not travel in a further region;
a centre point of the further region is at a distance from the predetermined point on the vehicle substantially equal to the minimum turn radius of the vehicle, in a direction substantially perpendicular to the velocity of the vehicle and substantially opposite to the direction of the centre point of the region from the predetermined point on the vehicle; and
a distance from the centre point of the further region to a point on a perimeter of the region is greater than or substantially equal to the minimum turn radius of the vehicle.
5. A method according to claim 4 , wherein the constraint that the vehicle may not travel in the further region is implemented by calculating:
∀
k
∈
[
0
,
…
,
N
steps
]
,
∀
m
∈
[
1
,
…
,
N
regions
]
(
r
x
(
k
)
-
r
0
x
-
R
min
v
nx
)
cos
(
2
π
m
N
regions
)
+
(
r
y
(
k
)
-
r
0
y
-
R
min
v
ny
)
sin
(
2
π
m
N
regions
)
≥
R
min
-
C
2
p
(
k
,
m
)
with
∀
k
∈
[
0
,
…
,
N
steps
]
∑
m
=
1
N
circ
p
(
k
,
m
)
=
N
regions
-
1
where:
v nx is the normalised velocity of the vehicle in an x-direction;
v ny is the normalised velocity of the vehicle in a y-direction;
r x (k) is a position of the vehicle from an origin in the x-direction;
r y (k) is a position of the vehicle from an origin in the y-direction;
r 0x is an initial position of the vehicle from an origin in the x-direction;
r 0y is an initial position of the vehicle from an origin in the y-direction;
N steps is a number of time-steps for the trajectory;
N regions is a number of corners of the polygon;
R min is a minimum turn radius of the vehicle;
p(k,m) is a binary decision variable; and
C 2 is a constant.
6. Apparatus for controlling a land vehicle having a curvature limit according to a trajectory, the apparatus comprising:
one or more processors and associated memory which, in combination, are configured to:
linearly approximate dynamics of a vehicle to produce a linear approximation that includes a state vector (x) that indicates and vehicle velocity with linearized variables that lack non-linear heading information;
determine, based on an identified starting position (p 0 ) for the vehicle, an identified desired terminal position (P) for the vehicle, and the linear approximation, a trajectory for the land vehicle that optimizes a linear time invariant system having a form {dot over (x)}=Ax+Bu against a cost function and with a first constraint, and without a second constraint, applied to the state vector (x) to determine an acceleration input (u) to be applied by the land vehicle to accomplish a portion of the trajectory;
determine, based on an identified starting position (p 0 ) for the vehicle, an identified desired terminal position (P) for the vehicle, and the linear approximation, a trajectory for the land vehicle that optimizes a linear time invariant system having a form {dot over (x)}=Ax+Bu against a cost function and with the second constraint and without the first constraint applied to the state vector (x) to determine an acceleration input (u) to be applied by the land vehicle to accomplish a portion of the trajectory; and
control the land vehicle to follow the trajectory;
wherein the second constraint provides that:
the vehicle may not travel in a designated region;
a centre point of the region will be at a distance from a predetermined point on the vehicle substantially equal to a minimum turn radius of the vehicle, in a direction substantially perpendicular to a velocity of the vehicle; and
a distance from the centre point of the region to a point on a perimeter of the region is greater than or substantially equal to the minimum turn radius of the vehicle;
wherein the first constraint includes at least one of:
a constraint on magnitude of acceleration of the vehicle;
a constraint on magnitude of velocity of the vehicle; and
a constraint that acceleration is larger when acting in a direction that is substantially perpendicular to velocity of the land vehicle than when acting in a direction that is substantially parallel to velocity of the land vehicle.