Navigation Functionalities for an Autonomous UAV Helicopter

4.2. TRAJECTORY GENERATOR 41
Calculation of the outer loop inputs
We can finally address the problem of generating the reference inputs for
the outer control loop. The inputs will be calculated in the form of control
errors (difference between the current helicopter state and the desired one)
as follows.
1. Calculation of the position error vector δp.
The position error vector is the difference between the control point
position pn cp and the helicopter position given by the INS/GPS system
pn heli . In order to be used in the outer loop control equations the
vector must be rotated from the navigation frame to the body frame
using the rotation matrix C b n:
δp b = C b n(p n cp − p n heli)
As explained in section 4.2.2, the control point position is calculated
using feedback from the helicopter position. The method does not
search for the control point along the whole path segment but it
remembers the value of the parameter s (e.g. the previous control
point) from the previous control cycle and starts the search from
there. By doing this, the search is very fast since the new control
point will not be far from the previous one (the control function is
called with a frequency of 50Hz). At the beginning of each segment,
the parameter value is set to zero.
Once the new value of s is found, the position error vector δp b can
be calculated together with the local path tangent T and curvature
K.
2. Calculation of the velocity error vector δv.
The velocity error vector is the difference between the target velocity
vtarg and the helicopter velocity given by the INS/GPS system v n heli .
The target velocity is obviously tangent to the geometric path. The
direction of the tangent vector is given by: