3D-Shortest paths for a hypersonic guilder in a heterogeneous environment P. Pharpatara, B. Hérissé, Y. Bestaoui 11/06/2015 - Seville, Spain
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Challenges
Motivations I
Trajectory planning is a high-demand algorithm for aerial vehicles ;
I
The shortest path between two vehicle states is a key element in many planning algorithms ; Dubins’ paths are mostly used in terrestrial vehicles ;
I
I
They are too simple for aerial vehicles model.
Objective I
2/19
Develop the efficient 3D-shortest path for aerial vehicles.
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Related works 2D paths I
Dubins’ paths [Dubins1957,Reeds1990,Boissonnat1991] : xfinal
xinit
xfinal
(i) CCC type
xinit
xfinal
xinit
(ii) CSC types
Figure: Dubins’ paths
3/19
I
Dubins’ path with constant wind effect[MacGee2005] ;
I
Dubins’ path in two property planes [Sanfelice2008] ;
I
Dubins’ path in an anisotropic environment [Dolinskaya2012] ;
I
Dubins’ path in heterogeneous environments [Hérissé2013].
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Related works 3D paths I
Shortest path are a helicoidal arc, a CSC path, a CCC path, or a degenerated form of these Dubins’ paths [Sussmann1995] ;
I
CCSC suboptimal paths are used for multiple UAVs path planning [Shanmugavel2007] ;
I
3D Dubins’ paths for aerial vehicles using Dubins’ car model [Hota2010] ;
I
3D Dubins’ paths in the presence of wind [Hota2014].
Proposed method I
Combination of techniques in [Hérissé2013] and [Hota2010] : I
4/19
3D Dubins’ paths in a heterogeneous environment.
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
System modeling
Environment model The air density ρ decreases exponentially with altitude z : ρ(z) = ρ0 e −z/zr ,
(1)
where ρ0 is the air density at standard atmosphere at sea level and zr is a reference altitude.
5/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
System modeling Vehicle model An interceptor during midcourse phase with zero wind assumption is used : I Gravity is neglected ; I Drag is ignored. v v eb1 e1
γ eb2 χ ev2
Cg eb ev3 3
k j O
i
Figure: Vehicle model
x˙ = v cos γ cos χ, y ˙ = v cos γ sin χ, z˙ = v sin γ, 1 ρ(z)SCLmax µ = vc(z)µ, γ˙ = v 2m χ˙ = v 1 ρ(z)SCLmax η = vc(z) η ,
2m cos γ cos γ where γ is the flight path angle, χ is the azimuth angle, m is the mass, S is the surface of reference, CLmax is the maximum p lift coefficient, µ, η are the normalized control inputs bounded by µ2 + η 2 6 1, and c(z) is the curvature of the vehicle path 6/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
System modeling Vehicle model An interceptor during midcourse phase with zero wind assumption is used : I Gravity is neglected ; I Drag is ignored. v v eb1 e1
γ eb2 χ ev2
Cg eb ev3 3
k j O
i
0 x = dx /ds = cos γ cos χ, 0 y = dy /ds = cos γ sin χ, z 0 = dz/ds = sin γ,
γ 0 = dγ/ds = c(z)µ, χ0 = dχ/ds = c(z) η , cos γ
Figure: Vehicle model where γ is the flight path angle, χ is the azimuth angle, m is the mass, S is the surface of reference, CLmax is the maximum p lift coefficient, µ, η are the normalized control inputs bounded by µ2 + η 2 6 1, and c(z) is the curvature of the vehicle path 6/19
IFAC-ACNAAV 2015
(2)
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Problem Statement
I
Non-linear and non-holonomic ;
I
Optimal trajectory from xinit to xgoal minimizing Z sf
J(x0 , xf , u) = I
7/19
ds;
(3)
0
Heterogeneous environment : decrease of the air density with altitude z, i.e. ρ(z) = ρ0 e −z/zr ⇒ Loss of maneuverability in high altitude.
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D-shortest paths using Dubins-like model Hypothesis The departure state x0 and the arrival state xf are sufficiently far from each other. Thus, only the CSC paths are considered.
Solution The 3D-shortest paths are generated using the combination of techniques in [Hérissé2013] and [Hota2010].
Approach I
Computation of the curves C in particular 2D plane : I I
I
8/19
A curve C1 starting from x1 in plane P1 ; A curve C2 ending at x2 in plane P2 .
Search for a line segment ` that connects both curves such that ` ∈ P1 and ` ∈ P2 .
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D-shortest paths using Dubins-like model Hypothesis The departure state x0 and the arrival state xf are sufficiently far from each other. Thus, only the CSC paths are considered.
Solution The 3D-shortest paths are generated using the combination of techniques in [Hérissé2013] and [Hota2010].
Approach I
Computation of the curves C in particular 2D plane : I I
I
8/19
A curve C1 starting from x1 in plane P1 ; A curve C2 ending at x2 in plane P2 .
Search for a line segment ` that connects both curves such that ` ∈ P1 and ` ∈ P2 .
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D-shortest paths using Dubins-like model Hypothesis The departure state x0 and the arrival state xf are sufficiently far from each other. Thus, only the CSC paths are considered.
Solution The 3D-shortest paths are generated using the combination of techniques in [Hérissé2013] and [Hota2010].
Approach I
Computation of the curves C in particular 2D plane : I I
I
8/19
A curve C1 starting from x1 in plane P1 ; A curve C2 ending at x2 in plane P2 .
Search for a line segment ` that connects both curves such that ` ∈ P1 and ` ∈ P2 .
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D-shortest paths using Dubins-like model Hypothesis The departure state x0 and the arrival state xf are sufficiently far from each other. Thus, only the CSC paths are considered.
Solution The 3D-shortest paths are generated using the combination of techniques in [Hérissé2013] and [Hota2010].
Approach I
Computation of the curves C in particular 2D plane : I I
I
8/19
A curve C1 starting from x1 in plane P1 ; A curve C2 ending at x2 in plane P2 .
Search for a line segment ` that connects both curves such that ` ∈ P1 and ` ∈ P2 .
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Computation of the curves C in particular 2D plane Definition of a particular 2D plane b φ ψ
z y p
dxp = cos θp , ds dzp (4) zp0 = = sin θp , ds dθp θp0 = = c(zp )up where up ∈ [−1, 1], ds xp0 =
P
p
e3
Vehicle model
e1
where θp = ∠(e1p , v) is a turning angle.
x
Environment model ρ(zp ) = ρ0 e −z/zr = ρ0 e −zp cos φ/zr . c(zp ) = c0 e
9/19
IFAC-ACNAAV 2015
−z/zr
= c0 e
−zp cos φ/zr
.
(5) (6)
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Computation of the curves C in particular 2D plane By differentiating θp0 with respect to s, we obtain θp00 = −
cos φ 0 θ sin θp . zr p
(7)
Then, cos φ zr 0 θp = θp0 − cos θp0 + 1 − 2 cos2 . z cos φ 2 r θ Let ζ = tan 2p . By substitution and trigonometric of ζ in equation (8), we have θp0
ζ 0 = A + Bζ 2 , where A and B are constant parameters.
10/19
IFAC-ACNAAV 2015
(8)
(9)
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Computation of the curves C in particular 2D plane 4 types of curves : I C1 if AB > 0 :
r ζ1 (s) =
" r
A tan A B
B s + arctan A
r
B ζ0 A
!# (10)
I C2 if AB < 0 :
" r r !# r A B B ζ2 (s) = tanh A s + arctanh ζ0 B
I C3 if A = 0 :
A
A
(11)
1 1 = − Bs ζ3 (s) ζ0
(12)
ζ4 (s) = ζ0 + As
(13)
I C4 if B = 0 :
11/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Computation of the curves C in particular 2D plane
50
Curve formulation
45 40
altitude (km)
35
zr zr (θp (ζ) − θp0 ) − (A + B)s xp (ζ) = cos φ cos φ 2
30 25
C2
20
zr
15 10
zp (ζ) = log cos φ
C1
5 0
−20
−10 0 10 horizontal distance (km)
20
Figure: Examples of arcs of maximum curvature
12/19
IFAC-ACNAAV 2015
1 + ζ0 A + Bζ 2 A + Bζ02 1 + ζ 2
θp (ζ) = 2 arctan ζ + k(s)π (14)
where k(s) is an integer depending on the distance s. Recall that ξ p = (xp , zp , θp )> on plane P.
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D path generation ` calculation I
Transform ξ p1 , ξ p2 in I frame : x1 = −zp1 sin φ1 cos ψ1 − xp1 sin ψ1 + x0 y1 = −zp1 sin φ1 sin ψ1 + xp1 cos ψ1 + y0 z1 = zp1 cos φ1 + z0 x2 = −zp2 sin φ2 cos ψ2 − xp2 sin ψ2 + xf y2 = −zp2 sin φ2 sin ψ2 + xp2 cos ψ2 + yf z2 = zp2 cos φ2 + zf where (φ1 , ψ1)and (φ2 , ψ2) are the orientations of the normal vectors b1 and b2 of the plane P1 and P2 , respectively.
I
A solver, for example, Newton method, is used to verify : F (l) = l − (ξ 2 − ξ 1 ) = 0
13/19
IFAC-ACNAAV 2015
(15)
(16)
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
3D path generation u1 u1 u1 u1
altitude (km)
14
= = = =
1, u2 = −1 1, u2 = −1 −1, u2 = 1 −1, u2 = −1
12 10
xf
8
x0
6 4 30 20
15 10
10
5
0
y (km)
0 −10
−5
x (km)
Figure: Four possible CSC paths between two states 14/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Simulation results
Configuration
15/19
I
State x = (x , y , z, γ, χ)> ;
I
Maximum curvature at sea level c0 = 1.4 × 10−3 m−1 ;
I
Reference altitude zr = 7500m ;
I
Departure from x0 ;
I
Arrival at xf .
IFAC-ACNAAV 2015
Con. & Pers.
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Simulation results Case 1 Maximum curvature at x0 : 1.4 × 10−3 m−1 ;
I
Maximum curvature at xf : 1.3 × 10−3 m−1 ;
I
path length 5.65km. altitude (km)
I
xf
0.4 0.2 0 4 3 2
y (km)
x0
1
2 1
0 0
x (km)
Figure: Case 1 : x0 = (0, 0, 0.005, 0, −π/3)> and xf = (2, 4, 0.5, 0, 2π/3)> 16/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Simulation results Case 2 I
Maximum curvature at x0 : 3.69 × 10−4 m−1 ;
I
Maximum curvature at xf : 4.96 × 10−5 m−1 ;
I
path length 59.84km. xf
altitude (km)
25
20
20
15
15 10 30
10
x0 25
20
15
y (km)
10
5
0
5 0
x (km)
Figure: Case 2 : x0 = (0, 0, 10, π/12, π/12)> and xf = (20, 15, 25, 0, 4π/3)> 17/19
IFAC-ACNAAV 2015
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Simulation results Case 3 I
Maximum curvature at x0 : 7.19 × 10−4 m−1 ;
I
Maximum curvature at xf : 4.97 × 10−5 m−1 ;
I
path length 37.04km. xf
altitude (km)
25
20
15
10
5
15 20
15
10
x0 5
y (km)
Figure: Case 3 : x0 = 18/19
IFAC-ACNAAV 2015
(0, 0, 5, π/2, 5π/6)>
0
10 5 0
x (km)
and xf = (15, 15, 25, −π/12, π/3)>
Introduction
System modeling
3D-shortest paths
Results
Con. & Pers.
Conclusions & Perspectives Conclusions I
The paths are more realistic than existing Dubins’ paths ;
I
Path generation is very efficient and fast ;
I
Easy to implement.
Perspectives I
3D path planning1 :
1
P. Pharpatara, B. Hérissé, Y. 3D trajectory planning of aerial vehicles using RRT*, Transaction on Control Systems Technology, submitted.
19/19
IFAC-ACNAAV 2015
Bestaoui. IEEE
