Navigation Functionalities for an Autonomous UAV Helicopter

A.1. PAPER I 79
The reference point on the nominal path is found
by satisfying the geometric condition that the
scalar product between the tangent vector and the
error vector has to be zero:
E • T = 0 (5)
where the error vector E is the helicopter distance
from the candidate control point. The control
point error feedback is then calculated as follows:
ef = E • T /|T | (6)
that is the magnitude of the error vector projected
on the tangent T . The control point is updated
using the differential relation:
dP = P ′ · ds (7)
Equation 7 is applied in the discretized form:
s(n) = s(n − 1) +
ef
| dP
ds (n − 1)|
(8)
where s(n) is the new value of the parameter.
Once the new value of s is known, all the path
parameters can be calculated.
The PF algorithm receives as inputs the target
velocity vt and the final velocity ve that the helicopter
must have at the end of the segment.
The path planner assigns the target velocity which
is related to the mission specification only. This
means that the path planner doesn’t have to take
into account any dynamic limitation of the helicopter
itself. The control law tries to keep the
target velocity, but when it is not compatible with
the local curvature of the path and the helicopter
performance limitations, the algorithm provides
an automatic limit on velocity. Velocity limitations
can be activated for two reasons: due to the
turn bank or the yaw rate limit of the helicopter.
In order to make a coordinated turn at constant
altitude, the flight mechanics provides the relation
between the velocity, the roll angle and the curvature
radius of the turn. Mechanical limits exist on
the maximum achievable swash plate angles, and
furthermore the helicopter envelope has currently
been opened up to φmax ± 8 deg for the roll angle
(relative to the hovering bank angle that is about
4.5 deg), and ωmax ± 26 deg/sec for the yaw rate.
Under the described limitations, it is possible to
calculate the maximum speed:
Vmax1 = � Rgφmax
(9)
Vmax2 = ω 2 maxR (10)
where R is the local curvature radius and g is the
gravity acceleration. The target speed assigned to
the path is compared with these two limits and
the lower speed is taken as target.
The braking algorithm continually checks the distance
between the helicopter and the end point of
the path. If the required acceleration to reach the
final target velocity exceeds a given value (currently
set to 1 m/s2 ), the current target velocity
is limited in order to maintain a constant deceleration.
In order to know the distance between the
helicopter and the end of the path, an estimate
of the final arc length of the curve has to be
calculated. The arc length of the spline between
the control point and the end point of the path is:
� Send �
lend = [x ′ (s)] 2 + [y ′ (s)] 2 + [z ′ (s)] 2 ds
S
(11)
If an analytical solution of the integral cannot be
found, a numerical method is used (rectangular
integration with for example 20 integration steps).
To gain computational time, the increments of the
flown path ln are subtracted from ltot (total length
of the path, i.e. lend at first iteration) to get a good
estimate of lend at each control cycle:
�
�
� [xn − xn−1]
�
ln = �
2 +
[yn − yn−1] 2 +
[zn − zn−1] 2
(12)
A path segment is considered finished when lend
is small enough. It should be emphasized that lend
is the arc length between the control point on
the curve and the end point of the path and not
between the helicopter and the end of the path.
This makes the system more robust with regard
to position error of the helicopter on the path.
5. CONTROL LAWS
The outer loop control (velocity and position
control) provides inputs for the YACS in order
to follow the path with the desired velocity. The
inner loop deals with the coupling dynamics of
the helicopter, so that the outer loop can handle
the four degrees of freedom as decoupled (i.e.
yaw rate, vertical velocity, pitch and roll angles).
The position and velocity error and centripetal
acceleration vectors are computed in the global
frame and then transformed into control inputs
after rotation in the helicopter’s body frame. The
acceleration vector is used as feed forward input
in the control law to improve the tracking in
the presence of path curvature. As regards the
acceleration vector, only the component in the
horizontal plane orthogonal to the path is used.
PD and PI compensators are used respectively
for position and velocity control. The algorithm
described in the previous section makes sure that
the position error vector is orthogonal to the
path and the velocity error vector is tangent to
the path. Given that the two error vectors are
orthogonal the velocity control doesn’t interfere
with the position control. The control equations
for the four channels are the following: