SMOOTHED HEX-GRID TRAJECTORY PLANNING USING

HELICOPTER DYNAMICS

Luk´aˇs Chrpa and Anton´ın Komenda

Agent Technology Center, Faculty of Electrical Engineering, Czech Technical University in Prague

Prague, Czech Republic

Keywords:

Path planning, Planning on grids, Model of dynamics, VTOLs, UAVs.

Abstract:

Considering Unmanned Autonomous Vehicles (UAVs) the planning tasks mainly consist of ﬁnding paths be-

tween given waypoints with respect to given constraints. In this paper we developed a path planning system for

ﬂying UAVs (VTOLs and CTOLs) built upon Hexagonal grids which also supports simple dynamics (handling

with speed). The planning system is additionally supported by a trajectory smoothing mechanism based on a

dynamics model of a helicopter. The model can be also used for the simulation of the helicopter movement.

1 INTRODUCTION

While controlling UAVs

we deal mainly with path

(trajectory) planning(Wang and Botea, 2009; Bot-

tasso et al., 2008; Meister et al., 2009). VTOLs

we will mainly focus on here, are usually helicopter-

like assets. Contrary to CTOLs

(aircraft-like assets)

VTOLs can change their direction or altitude with-

out changing x or y coordinate. We should mention

a state-of-the-art system for air trafﬁc control Agent-

Fly (

Siˇsl´ak et al., 2008). Plans (paths) provided by

the AgentFly are represented by sequences of maneu-

vers (unit movements). Another state-of-the-art sys-

tem deals with path planning for VTOLs (Wzorek

and Doherty, 2006). In the system, there is used

a path planning algorithm based on Probabilistic

Roadmaps (Kavraki et al., 1996) which results in a

sequence of waypoints interpolated by splines.

In this paper, we introduce our path planning sys-

tem for VTOLs (its modiﬁcation can be used also for

CTOLs). The planner is built upon Hexagonal grids,

A* algorithm (Hart et al., 1968) is used as a planning

procedure. The planner supports simple dynamics in-

corporating speed constraints which are, for instance,

good for detecting whether a certain turn maneuver is

applicable. Using grids instead of irregular (but more

realistic) maneuvers used in AgentFly brings a signif-

icant increase of performance because the node com-

Unmanned Autonomous Vehicles

Vertical Take-Off and Landing UAVs

Classical Take-Off and Landing UAVs

parisons (frequently done during the planning pro-

cess) is much more easier for grids. Using ‘spline

planner‘ does not seem to be possible while taking

into account speed limits, because in every point the

curvature must be somehow limited depending on the

previous part of the spline curve.

Methods for trajectory smoothing and optimal tra-

jectory planning are covered by ﬁeld of control theory.

However, there is a lack of the state-of-the-art works

merging classical grid or symbolic planning and tra-

jectory smoothing based on the dynamics of the vari-

ous vehicles. Method for real-time trajectory smooth-

ing is proposed by (Anderson et al., 2005). Spatial

waypoints without speed constraints are input of the

method and the output is a generated trajectory for the

vehicle. An approach sharing an idea of the planning

maneuvers is presented in (Bottasso et al., 2008). The

trajectory smoothing process uses motion primitives

which are an abstraction of the movement maneuvers.

2 PLANNING FOR VTOLS

Our basic model, simply, deals with hex-grid path

planning. Since the hexagonal lattice is only 2D, we

must consider ﬂight levels. Then, the discretization of

the space forms ‘honeycomb‘.

The planning problem deals with ﬁnding a path

from the starting to ending waypoint, following the

directions (if deﬁned) and avoiding the No-ﬂight-

zones (NFZs). Let us deﬁne a set of primitive ma-

629

Chrpa L. and Komenda A..

SMOOTHED HEX-GRID TRAJECTORY PLANNING USING HELICOPTER DYNAMICS.

DOI: 10.5220/0003184106290632

In Proceedings of the 3rd International Conference on Agents and Artiﬁcial Intelligence (ICAART-2011), pages 629-632

ISBN: 978-989-8425-40-9

 2011 SCITEPRESS (Science and Technology Publications, Lda.)

neuvers (we allow ‘diagonal‘ moves):

Straight — Moves to adjacent (also ‘diagonally ad-

jacent) hexagon according to direction.

Turn — Changes the direction (to the left or right)

by an angle π/6, π/3 and π/2.

Pitch — Moves to adjacent upper (resp. lower) ﬂight

level.

Additionally, there are several restriction that we

applied for plan generation. Every turn maneuver

must be followed by the straight maneuver. Every

pitch maneuver must be applied between two straight

maneuvers. Planning procedure is done by A* al-

gorithm, where the actual distance from the start-

ing waypoint g and the estimated cost to the ending

waypoint (heuristic) h are computed in every open

node. The heuristic h is computed as Vancouver dis-

tance (Yap, 2002) (analog to Manhattan distance). If

‘diagonal‘ moves are allowed, then the heuristic h is

inadmissible but the optimality of the plans is affected

very slightly.

We introduce our model with simple dynamics

where we have to take into account a new parameter

- speed, since the basic model works well only for

UAVs that move slowly.

We extend the planning problem by a basic speed

model, where we deﬁne maximal acceleration and

deceleration, minimal and maximal speeds with re-

spect to particular maneuvers. From the physics it is

well known that a minimal turn radius grows quadrat-

ically as a speed grows. We also can deﬁne an initial

speed (in the starting waypoint) and voluntarily a tar-

get speed interval (in the ending waypoint).

Algorithm 1: Computing a speed interval for a newly

opened node.

1: {Upper indices − (resp. +) stand for minimal (resp. maximal) speed

values.}

2: if maneuver is the Straight maneuver then

3: v

−

curr

:= MAX



−

)

− 2as



4: v

curr

:= MIN



)

+ 2as



5: end if

6: if maneuver is the Turn or Pitch maneuver then

7: if hv

−

i ∩ hv

−

man

i =

0 then

8: return null {Speed constraint cannot be satisﬁed for the Turn or

Pitch maneuver}

9: end if

10: v

−

curr

:= MAX(v

−

man

)

11: v

curr

:= MIN(v

man

)

12: end if

13: return v

−

curr

Considering speed as a new dimension might re-

sult in an explosion of the state space. To avoid this

we introduce speed intervals that represent a range of

Algorithm 2: Propagation of speed intervals.

1: {Upper indices − (resp. +) stand for minimal (resp. maximal) speed

values.}

2: count

min

:= count

max

:= 0

3: minspd := v

−

curr

; maxspd := v

curr

4: Select the predecessor node as a current node

5: while v

−

curr

< minspd ∨ v

curr

> maxspd do

6: Push the current node into Stack

7: if maneuver is not the Turn or Pitch maneuver then

8: maxspd = MIN



curr

maxspd

+ 2as



9: if maxspd 6= v

curr

then

10: count

max

+ +

11: end if

12: minspd = MAX



−

curr

minspd

− 2as



13: if minspd 6= v

−

curr

then

14: count

min

+ +

15: end if

16: end if

17: Select the predecessor node as a current node

18: end while

19: while Stack is not empty do

20: Pop the element from the stack as a current node

21: if maneuver is not the Turn or Pitch maneuver then

22: if count

max

> count

min

then

23: maxspd :=

maxspd

− 2as; count

max

− −

24: else if count

max

< count

min

then

25: minspd :=

minspd

+ 2as; count

min

− −

26: else

27: maxspd :=

maxspd

− 2as; count

max

− −

28: minspd :=

minspd

+ 2as; count

min

− −

29: end if

30: end if

31: v

curr

:= MIN(v

curr

,maxspd)

32: v

−

curr

:= MAX(v

−

curr

,minspd)

33: end while

feasible speed values in every explored node. Speed

can be changed only during the Straight maneuver ac-

cording to the speed model. During the Turn or Pitch

maneuver the speed remain constant. Any speed lim-

itation (for the Turn or Pitch maneuver) can be eas-

ily checked, i.e., if a non-empty subset of the partic-

ular speed interval satisfy the particular limits. For

the formal description, see alg. 1. We have to back-

propagate speed changes (see alg. 2) to follow the dy-

namics, for instance, the VTOL must begin to slow

down at some distance before performing the Turn

maneuver.

A feasible plan consists, like in the basic model,

a sequence of points and directions (in these points).

Maximal values of speed intervals in every point can

be set as (exact) speed values. Then, we can simply

compute time-stamps in all the points.

ICAART 2011 - 3rd International Conference on Agents and Artificial Intelligence

630

3 TRAJECTORY SMOOTHING

USING A MODEL OF

HELICOPTER DYNAMICS

The model of helicopter dynamics serves two pur-

poses. Firstly, it is used as a smoothing method for

the resulted plans from the hex-grid planner (see Sec-

tion 2). Secondly, it is used during the simulation of

the helicopter movement. These two cases differ in

two main aspects. The smoothing process runs the

model using larger time steps (improving the compu-

tational duration). On the other hand, the simulation

phase may include additional errors in the movement

caused by various real-world effects (e.g. wind, im-

perfections of the asset hardware, sensory errors and

glitches, biased control and others).

We consider 6 DOF

model consisting of three

spatial and three rotational axes. The spatial position

is described by translation vector p = (x,y,z), and the

rotation is described by three euler angles ϕ,θ,ψ in a

quaternion form q = (q

)

Furthermore, both the spatial and the rotational

axes are completed by their ﬁrst and second derivates,

i.e., velocities and accelerations. Spatial velocity and

acceleration can be simply derived as follows:

= v, p =

vdt, p = p

+ vt.

The spatial acceleration respects the same pattern.

The ﬁrst derivate of the rotation (angular veloc-

ity ω

ω) described in a quaternion can be derived using

differential equation for varying quaternion with an-

gular velocity described in an augmented quaternion:

ω(t) = (0,ω

(t),ω

(t))

as follows:

q∗ω

ω, q =

q∗ω

ω, q = q

exp



ωt



The second derivate (angular acceleration α

α) is based

on a vector derivation of the augmented quaternion

describing the angular velocity ω

ω.

The introduced equations describe only dynam-

ics of a mass object in space. Following formulas

describe a simpliﬁed physical model of a helicopter

based on the air lift formula L(η) =

ρv

(η),

where L[N] is the lift force, ρ[

] is air density,

/s] is velocity of the rotor disk causing the lift

force, S[m

] is area of the rotor disk, and C

(η) is

coefﬁcient of lift. The lift coefﬁcient C

is a function

of the attack angle η of the rotor blades and it is for

the purposes of the model linearized by partially lin-

ear function with an ascending part with coefﬁcient k

and a descending part with coefﬁcient k

Degrees of freedom

(η) =

(

|η| <

π : ηk

|η| ≥

π : (η−

π)k

πk

The spatial acceleration of the object with a

weight m is affected by two forces (i) gravity, and

(ii) the lift force of the main rotor which is param-

eterized by the collective rotor tilt ν and computed

as a = (0,0, 0,−g)

+ q(0,0,0,

L(ν)

)

∗

. The aug-

mented vector a is deﬁned as a = (0,a)

, q

∗

repre-

sents an inversion of a quaternion q, and qpq

∗

de-

scribes rotation of augmented vector p by a quater-

nion q.

The angular acceleration is affected by cyclic

rotor tilt in two dimensions σ

,σ

and anti-

torque tail rotor tilt τ in a following ways α

α =

(

(σ

)

(σ

)

,0) + (0,0,

(τ)

). The radius of the

main rotor is denoted as r, distance of the tail rotor

from the main rotor axis is denoted as r

. The lift

force of one half of the main rotor used by the cyclic

tilt is denoted as L

and the lift force caused by the

tail rotor is denoted as L

. The parameters for the par-

ticular lift formulas differs in area of the rotor S and

mean velocity of the rotor v. The caused lift force of

the rotor cyclic is positioned in the center of the rotor

radius

3.1 Model Stabilization

The only explicit stabilization is used for horizontal

stabilization of the model. The stabilizers ensure the

model will always have tendency to stay horizontally

even. The stabilizers are two PID

controllers. Each

controller stabilizes the model in one axis, i.e. roll ϕ

and pitch θ. The target state of the model rotation is

zero angles of rotation. The regulation equations are

based on the equation of ideal PID controller and they

are discretized and used in iterative manner.

3.2 Movement Regulation

Movement of the model in the main four axes (spa-

tial axes x,y,z and rotational axis ψ) is handled by

four movement PID controllers. On the contrary of

the stabilization controllers, the movement controllers

are designed to fulﬁll requested height, velocity, di-

rection, and yaw rotation.

The ﬁrst regulated value is the height z of the

model to target height z

. It is based on PID con-

troller. The affected action control is the collective

rotor tilt ν, i.e., strength of the main rotor for climb-

ing/descending.

proportional–integral–derivative

SMOOTHED HEX-GRID TRAJECTORY PLANNING USING HELICOPTER DYNAMICS

631

The second two controllers affect the rotor cyclic

providing horizontal movement of the helicopter. The

formulas are similar to the equation of the height con-

troller and differs in the used parameters and the error

deﬁnition. The σ

collective tilt is regulated by er-

ror value (v

− v

) and σ

by error value (v

− v

The used parameters are P

. The current veloc-

ity v is regulated towards the target velocity v

in the

required direction to the target point (typically a tra-

jectory waypoint). The waypoints are switched after

reaching a predeﬁned distance to the next waypoint.

The fourth controller is used for pointing of the

head of the model towards the target point. Although

the model can reach any position using only the previ-

ous three spatial axes, the yaw rotation is important if

the helicopter carries an orientation dependent sensor

(e.g. a range ﬁnder, or a camera). The torque con-

troller is also PID with different identiﬁed constants.

3.3 Trajectory Smoothing using the

Model

The model used for the smoothing of the trajectory

produced by the hex-grid planner (see Section 2) uses

all the introduced controllers together with the model

of helicopter dynamics. The plan begins at ground

and increases the height together with forward move-

ment towards the target waypoint. During the ﬂight

a NFZ is avoided provided that the hex-grid planner

planned the waypoint around the zone.

All the parameters of the controllers (P

,...,D

) were identiﬁed using the Ziegler-Nichols

method. The model parameters including weight, ro-

tor area, distance to the tail rotor and others are based

on the Skeldar 200 VTOL from SAAB Aerosystems.

4 CONCLUSIONS

We proposed a path planner based on hexagonal

grids for VTOLs supported by a trajectory smooth-

ing mechanism based on a dynamics model of a heli-

copter. The planner can also handle itself simple dy-

namics considering a simple speed model, where we

deﬁne certain constraints. The planner-based dynam-

ics is reﬂected by the smoothing mechanism generat-

ing a trajectory usable by a physical model of a heli-

copter. The computational complexity of the smooth-

ing method is linear as the process is based on inte-

gration with a constant step over the space.

For the future we should study how to add a time

model, now we can only compute time intervals (ac-

cording to speed intervals) in every point of the trajec-

tory. It could be formulated as a constraint satisfac-

tion problem and run as a postprocessing task on the

found trajectory. We should also investigate another

problem - collision avoidance in multi-agent systems.

It is necessary to investigate the possibilities of efﬁ-

cient replanning while some collision threatens.

ACKNOWLEDGEMENTS

This work was supported by Czech Ministry

of Education, Youth and Sports under Grant

MSM6840770038 and by U.S. Army Grant

W911NF-08-1-0521 and W15P7T-05-R-P209

under Contract BAA8020902.A.

REFERENCES

Anderson, E. P., Beard, R. W., and McLain, T. W. (2005).

Real time dynamic trajectory smoothing for uninhab-

ited aerial vehicles. IEEE Transactions on Control

Systems Technology, 13(3):471–477.

Bottasso, C. L., Leonello, D., and Savini, B. (2008).

Path planning for autonomous vehicles by trajectory

smoothing using motion primitives. IEEE Transac-

tions on Control Systems Technology, 16(6):1152–

1168.

Hart, P., Nilsson, N., and Raphael, B. (1968). A formal

basis for the heuristic determination of minimum cost

paths. IEEE Transactions on Systems Science and Cy-

bernetics, 4(2):100 –107.

Kavraki, L. E., Svestka, P., Latombe, J. C., and Overmars,

M. H. (1996). Probabilistic roadmaps for path plan-

ning in high-dimensional conﬁguration spaces. IEEE

Transactions on Robotics and Automation, 12:566–

580.

Meister, O., Frietsch, N., Ascher, C., and Trommer,

G. F. (2009). Adaptive path planning for vtol-uavs.

Aerospace and Electronic Systems Magazine, IEEE,

24(7):36–41.

Siˇsl´ak, D., Pˇechouˇcek, M., Volf, P., Pavl´ıˇcek, D., Samek, J.,

Maˇr´ık, V., and Losiewicz, P. (2008). Defense Industry

Applications of Autonomous Agents and Multi-Agent

Systems, chapter AGENTFLY: Towards Multi-Agent

Technology in Free Flight Air Trafﬁc Control, pages

73–97. Birkhauser Verlag.

Wang, K.-H. C. and Botea, A. (2009). Tractable multi-agent

path planning on grid maps. In Proceedings of IJCAI,

pages 1870–1875.

Wzorek, M. and Doherty, P. (2006). Reconﬁgurable path

planning for an autonomous unmanned aerial vehicle.

In Proceedings of ICAPS, pages 438–441.

Yap, P. (2002). Grid-based path-ﬁnding. In Canadian Con-

ference on AI, pages 44–55.

ICAART 2011 - 3rd International Conference on Agents and Artificial Intelligence

632