Method of aerial vehicle trajectory planning
View Patent ↗Methods of aerial vehicle trajectory planning are provided. The method is used where the aerial vehicle is in an area with one or more obstacles therein and the aerial vehicle has a minimum turn radius. The methods involve finding safe flight corridors, which are constructed from sequences of triangles using constrained Delaunay triangulation of the feasible space. A quadratic Bézier curve corresponding to each triangle in the flight corridor is defined. The trajectory is composed of quadratic Bézier curves that are aligned end-to-end which satisfy the vehicle motion constraints. The method described herein may also be used for path planning.
1 . A method of planning a trajectory for an aerial vehicle in an area and following the trajectory from a start location to an end location, which area has one or more obstacles therein, wherein said aerial vehicle has a minimum turn radius, said method comprising the steps of:
a) generating a safe corridor to account for obstacles in the area in which the aerial vehicle will be traveling, said step of generating a safe corridor comprising representing the flight corridor as a series of triangles, said triangles having three edges, wherein obstacles are defined as holes in the area between said triangles;
b) defining a quadratic Bézier curve (QBC) corresponding to each triangle in the flight corridor, wherein each quadratic Bézier curve has a pair of ends and a plurality of control points, wherein said control points include an initial control point and a terminal control point, and bounding each quadratic Bézier curve to lie within the corresponding triangle, wherein said quadratic Bézier curves include an initial Bézier curve and a final Bézier curve, wherein said initial control point and said terminal control point of the Bézier curves other than the initial control point of the initial Bézier curve and the terminal control point of the final Bézier curve are constrained to lie on the triangle edges, and wherein the radius of curvature along all portions of said quadratic Bézier curves is greater than or equal to the minimum turn radius of the aerial vehicle;
c) constraining the terminal control point on the final Bézier curve to align with the end location;
d) aligning said quadratic Bézier curves end-to-end to form a trajectory comprising a path having a length, wherein the tangent to the path is continuous along the length of the path;
e) utilizing a nonlinear program to simultaneously solve for the control points of all of the quadratic Bézier curves to generate a trajectory for said aerial vehicle; and
f) controlling said aerial vehicle from said start location to said end location along said trajectory generated in steps a)-e).
2 . The method of claim 1 wherein the method is used by said aerial vehicle to plan a trajectory among obstacles that determines a time of arrival to a destination.
3 . The method of claim 1 wherein steps a) to e) and the trajectory are all computed onboard an aerial vehicle while the aerial vehicle is in flight.
4 . The method of claim 1 wherein the method is used to plan and follow a trajectory for minimum time rendezvous with a moving aerial object.
5 . An aerial vehicle comprising:
a body;
a propulsion system joined to said body;
an on-board computing device inside said body for planning the trajectory of the aerial vehicle, wherein said on-board computing device plans a trajectory for said aerial vehicle, wherein said computing device is configured to:
generate a safe corridor to account for obstacles in the area in which the aerial vehicle will be traveling by representing the flight corridor as a series of triangles, said triangles having three edges, wherein obstacles are defined as holes in the area between said triangles;
define a quadratic Bézier curve corresponding to each triangle in the flight corridor, wherein each quadratic Bézier curve has a pair of ends and three control points, wherein said control points include an initial control point and a terminal control point, wherein said quadratic Bézier curves include an initial Bézier curve and a final Bézier curve, wherein said initial control point and said terminal control point of the Bézier curves other than the initial control point of the initial Bézier curve and the terminal control point of the final Bézier curve are constrained to lie on the triangle edges;
align said quadratic Bézier curves end-to-end to form a trajectory comprising a path having a length wherein the tangent to the path is continuous along the length of the path;
utilize a nonlinear program to simultaneously solve for the control points of all of the quadratic Bézier curves; and
control the aerial vehicle to follow the trajectory to a destination.
6 . The aerial vehicle of claim 5 comprising a pair of fixed wings joined to said body.
7 . An unmanned aerial vehicle according to claim 5 .
8 . The method of claim 1 wherein there is a closed form solution for the maximum curvature and the length of the QBCs.
9 . The method of claim 8 wherein the closed form solution of the maximum curvature K max , of a QBC as a function of the control points depends on the position of p 1 as shown below:
κ
max
=
{
p
1
m
3
A
t
2
,
i
f
p
1
∉
C
1
⋃
C
2
,
A
t
p
0
p
1
3
,
i
f
p
1
∈
C
1
,
A
t
p
1
p
2
3
,
i
f
p
1
∈
C
2
.
where p 0 ,p 1 ,p 2 are the control points of a QBC, P(τ); the mid-point of the line joining p 0 and p 2 is represented by m; A t is the area of the triangle Δp 0 ,p 1 ,p 2 ; C 1 and C 2 are circles with diameters p 0 m and p 2 m respectively; and, the symbol ∉ indicates no set membership and reads “is not an element or member of” or “does not belong to”.
10 . The method of claim 8 wherein the length of a QBC can be expressed as an analytical function of its control points, and is given below:
L
b
=
1
8
A
3
2
[
4
A
3
2
W
+
2
A
B
(
W
-
C
)
+
(
4
CA
-
B
2
)
log
❘
"\[LeftBracketingBar]"
2
A
+
B
A
+
2
W
B
A
+
2
C
❘
"\[RightBracketingBar]"
where
W
=
A
+
B
+
C
,
A
=
4
(
a
x
2
+
a
y
2
)
,
B
=
4
(
a
x
b
x
+
a
y
b
y
)
,
C
=
b
x
2
+
b
y
2
,
and
a
=
p
0
-
2
p
1
+
p
2
,
b
=
2
p
1
-
2
p
0
,
where p 0 , p 1 , and p 2 are the control points of a QBC, and a and b are functions of the control point coordinates, wherein a x , a y , b x , and b y are the x-coordinate and y-coordinate values of a and b, respectively.