Control Algorithms



The vehicle control algorithm was designed to control a rotorcraft UAV (a helicopter), but could be easily adopted for a fixed-wing aircraft (in fact, such control scheme would be much simplified, due to less degrees of freedom an aircraft has compared to a helicopter).

We decided to decompose the complex control problem into independent subproblems and solve them separately, if possible. This attempt resulted into the hierarchical control scheme, depicted in the first figure. Right at the start, we decided to completely separate the altitude control from the rest, essentially decomposing the 3D problem into separate 1D and 2D subproblems. Hence there is an independent Altitude Control Layer (ALCL), controlling the altitude of the vehicle, and a completely separated hierarchical structure, taking care of (2D) trajectory tracking (in terms of geographical position, i.e. latitude and longitude). Since both layers could be synchronized in time, this solution does not limit the ability of the system to track a complex 3D trajectory in any way.

For a helicopter, this approach is very convenient, since the altitude might be controlled by a simple SISO PID controller driving the collective and throttle actuators (assuming that tilt, meaning roll and pitch, angles of the vehicle are reasonably limited and the vehicle is always in the upright position). Of course, this simplistic assumption excludes any kind of aerobatics, but it is sufficient for any “reasonable” flight envelope. There are interdependencies between the tilt angles, acceleration of the vehicle and collective settings, but can be safely neglected for reasonably limited tilt angles (by “reasonable” we mean 20°-25°). These neglected interactions can be viewed as an additional small disturbance, which can be safely handled by altitude and velocity controllers. They could also be compensated for by introducing some simple feed-forward branches between roll, pitch, velocity and altitude controllers, but it is disputable whether this solution would bring any measurable benefits.

The “2D” control scheme is hierarchically structured, consisting of five separate layers. The first layer (Angular Rate Control Layer, ARCL) is responsible for the angular rates stabilization in the three vehicle axes. The second layer (Attitude Control Layer, ACL) serves for the attitude stabilization, while the third (Velocity Control Layer, VCL) controls the vertical and horizontal velocities of the vehicle. The fourth layer (Position Control Layer, PCL) is used to to stabilize the vehicle position in space. The uppermost layer (Trajectory Tracking Layer, TTL) is responsible for the vehicle guidance along a pre-set trajectory.

This control scheme, as envisioned here, represents the “ultimate goal” and is not completely implemented in the current software revision. A little simplified control scheme, omitting the VCL, is in place - Meaning that the PCL output is used directly as a command for ACL. This is possible since there is a direct correlation between the two attitude angles (roll and pitch) of a helicopter and its movement. Indeed, a forward tilt makes a helicopter fly forwards, left tilt to the left and so on. It is therefore possible to use the position difference directly to compute desired attitude, skipping the velocity layer. Of course, this strategy is very restrictive - strict limitations must be enforced to maximal allowed roll and pith angles, in order to prevent excessive accelerations of the vehicle, and very conservative position controller must be used. This approach sacrifices a lot of performance, because the acceleration and maximum speed of the vehicle must be severely limited (by limiting allowed tilt angles). Otherwise, it would be nearly impossible to set the position controller properly (at least a PID controller), because the response of the system would be drastically non-linear - since tilt makes the vehicle accelerate in corresponding direction (until steady state is reached between the corresponding portion of the thrust and drag), not to fly at a steady pace. It is clear that great performance benefit could be gained by implementing VCL, and it is not so hard to do so - provided that there is reliable velocity measurement available 8-), which is not the case with RAMA, at least not in its current incarnation. The only velocity measurement is currently provided by the GPS, available only at 1Hz sampling rate, which is clearly not sufficient for any meaningful velocity control. That is the reason why we opted for this far-from-optimal solution temporarily, until the data acquisition problem could be solved.


The same holds for the Kalman filter, depicted in the block diagram. It is meant for the sensor data fusion, in order to determine all necessary input data for the control algorithms. In order to do this, a complete “navigation” Kalman filter would be needed, for the higher-level algorithms require data that are not directly measurable (attitude, velocities and space coordinates). At this point of development, our Kalman filter only provides the attitude angles (and angular rates estimation). Currently, the data from the Kalman filter are used solely for the ACL layer; the other layers rely on “raw” sensor data. The angular rates are measured directly by the IMU, filtered (using only a first order FIR filter, to avoid excessive latency), and fed to the ARCL. The other layers (PCL, TTL and ALCL) currently use raw GPS data.

Current situation, as implemented in the software, is shown in the second figure.

In the next sections, the current state of development in the fields of measurements and sensor data fusion, as well as control algorithms, will be described.

Measurements and Sensor Data Fusion

For the two lowermost layers currently implemented, angular rates and attitude angles of the vehicle are necessary as inputs. The angular rates are measured directly by the gyroscopes, and therefore pose no problem. No Kalman filter or other algorithm for sensor fusion was necessary when only the ARCL was functional.

For the ACL, attitude angles are needed, and obviously those can be determined only indirectly, using some sort of sensor data fusion. The sensor data fusion and attitude determination is the work of Marek Peca, who is the author of algorithms mentioned in this section.

A Kalman filter would be the most obvious way to go, but for start, it was decided to develop a simple analytical computation as the very first method. The reasons behind this were the following: To obtain a better insight and to have some sort of workable solution, simple to debug; to have a benchmark for the Kalman filter development; and to perform a feasibility study that would confirm that the data provided by the sensors are good enough and that the angles could be reliably determined. A Kalman filter is not so simple to debug and could also mask various systematic errors, that could come into play later. Also, a Kalman filter is very good in “masking” occasional corrupted samples, but on the other hand could “get lost” on some bad data and its recovery might be problematic, even once the data pick up again. In this case, it is very convenient to have some sort of simple solution, working as a reference, which has no memory (operates only with a single sample) and therefore recovers immediately once good data are received. This could be used to “reset” the Kalman filter and help it out of such a trap.

Analytical Computation of Attitude Angles

Euler angles are used to express the rotation of the vehicle body in space. There is the possibility of the gimbal lock occurance for the 90° tilt angles, but it does not matter for the attitude stabilization. The gimbal lock would be a problem for trajectory reconstruction and will have to be solved once the sensor fusion algorithm will be used for navigation. The obvious solution is to use the Quaternions instead of the Euler angles. The Euler angles were chosen as an intermediate solution, because they are simple, intuitive and provide a direct insight into the algorithm, which Quaternions do not.

The attitude is computed using only the accelerometer and magnetometer data, i.e. the acceleration and magnetic intensity measurements in three vehicle axes. It is not possible to use any other data (i.e. the angular rates), because the resulting system of equations would be overspecified and therefore analytically unsolvable.

It is assumed that the vehicle is not accelerating much, so the acceleration measurements are given solely by the earth gravity vector. Both acceleration and magnetic intensity measurements are filtered, in order to get rid of the vehicle-induced accelerations and noises, affecting both accelerometers and magnetometers.

The first rotation (from the earth to the body frame) is chosen to be around the z axis, so it is independent on the acceleration measurements (the earth frame is chosen so that the gravity force vector coincides with the z axis and is zero in x and y axes, so the gravity measurement in the frame rotated around the z-axis is the same as in the original frame). So the first rotation angle is determined solely from the magnetic intensity measurements. The other rotations are chosen around the x' and the z'' axes and could be determined using solely the gravity measurements. Using this process, the rotation matrix R can be determined using following algorithm (the measurements are taken in the body frame, so the rotation matrix is computed in order from the body frame to the earth frame):

The first rotation from the body frame around the z'' axis can be computed using gravity measurements:


where Θ1 is the angle of the first rotation and gx, gy are the gravity measurements in the x'' and y'' body frame axes. Now the second rotation could be computed as follows:


where Θ2 is the angle of the second rotation around the x' axis and gz is the gravity measurement in the z'' (and consequently also in the z') axis, because the first rotation did not affect the z'' axis measurement.

Now the rotation matrix R12, representing the first two rotations, can be constructed:


Where s1 = sinΘ1, c1 = cosΘ1, s2 = sinΘ2 and c2 = cosΘ2. The next step is to transform the magnetic intensity measurements into this newly constructed frame, using the R12 matrix:


Where h is the column vector of magnetic intensity measurements hx, hy and hz in the body frame and v is the resulting column vector of the measurements, transformed into the doubly-rotated inter-frame. Now the last rotation is in order, computed using the transformed magnetic intensity measurements:


where Θ3 is the last Euler angle. Let us now construct the last rotation matrix Rz, representing the last rotation only:


where s3 = sinΘ3 and c3 = cosΘ3. The complete rotation matrix R can be obtained by:


The condition when the denominator of one of the arctan function arguments in equations (1), (2) and (5) becomes zero corresponds to the gimbal lock condition in that attitude angle, meaning that a degree of freedom has been lost. This is a non-issue for this algorithm, because the C function atan2 is used to compute the arctan function, which can handle the zero-denominator condition; it returns 0 or π in that case, depending on the sign of the nominator. The corresponding attitude angle, to which the degree of freedom was lost, would then be computed accordingly. Therefore, this algorithm can safely handle the gimbal lock singularities, inherently present in the Euler angles attitude representation.

Unfortunately, the Euler angles Θ1, Θ2 and Θ3 do not directly correspond to the yaw, pitch and roll (YPR) attitude angles of the vehicle. This is because the z-x-z rotation was used for the sake of derivation simplicity, while the YPR attitude angles correspond to the z-y-x rotation. It is therefore necessary to convert the angles Θ1, Θ2 and Θ3 into the YPR angles ψ, Φ and θ:


The conversion (8) can be found in the Robotic Toolbox for MATLAB.

Kalman Filter for Attitude Angles

Unfortunately, no documentation was completed yet for the Kalman filter, although the source codes are available in the software tarball. The documentation will be (hopefully) added sometime in the future.

SISO PID Controller Structure


RAMA currently utilizes SISO (Single Input Single Output) PID controllers at all levels of control. The controller scheme was proposed by Ondřej Holub and implemented by Ondřej Špinka, and is further developed in close cooperation of both authors. It is our original PID implementation, inspired by [1]. On top of standard PID control law it utilizes some additional techniques, like reference filtering and weighting, feed-forward branch and separate D-component filtering and restriction. It is also equipped with the bumpless control transition block, useful when taking over of a manually-controlled actuator.

Reference weighting in the P branch helps to suppress the non-linear gain of the controlled system, while in the D branch it serves to restrain the reference kick.

The controller parameters are as follows:

k, i, d, m - Proportional, Integral, Derivative and Feed-Forward gains

b, c - Reference weighting coefficients for proportional and derivative components

w - Reference filter cut-off frequency

n - D filter cut-off frequency

h - Sampling period

The difference computation of the reference r and measurement y are divided into two separate branches, to allow for the reference weighting (as described in [1]). Hence, the controller has separate inputs for r and y, instead of a single input for the control error e. A low-pass frequency filter is embedded in both D branches, to get rid of the high-frequency noise possibly superposed on the input signals. The D branch contribution to the output sum is bounded by a static restrictor.

The I branch is fairly conventional. Its contribution to the output is limited by a static anti-windup filter. In the Manual Control Mode (MCM), the bumpless transition branch sets the summator value of the I branch so that the controller output precisely matches the manual action. Therefore, when the mode switch occurs and the Automatic Control Mode (ACM) is engaged (the controller takes over), no sudden kick to the actuator can happen.

The P branch is also conventional, only the reference r is weighted by the parameter b before the control error e is computed.

The output of the controller is limited by a static restrictor as a safety measure, to prevent the action to exceed the saturation limits of the actuator (although this should not happen if the parameters are set properly).

Control Loops

Angular Rate Control Layer

The Angular Rate Control Layer (ARCL) consists of three separate, independent SISO PID controllers, one for each axis (yaw, pitch and roll). The angular rates of a rotorcraft are completely independent and each can be controlled directly by a separate actuator, which makes the situation particularly simple from the architectural point of view. The yaw rate is controlled by the tail rotor thrust, the pitch rate via the longitudinal cyclic control and the roll rate through the lateral cyclic control.

Control parameters in respective control loops are very different, because the vehicle dynamics is very diverse in each axis. The yaw has the fastest dynamics, because the moment of inertia of the vehicle is the smallest in this axis. Small rotorcraft can typically spin at rates as fast as 350-450°/s and the vehicle step response is very crisp - it can accelerate/decelerate significantly in that axis (see section Yaw Rate Control for details). The yaw rate dynamics is the determining parameter for the sampling frequency. At the beginning of the RAMA project, the sampling frequency of 32Hz was deemed sufficient, but it turned out that the yaw rate is hardly controllable at this frequency. It was therefore increased to 64Hz (the highest frequency the IMU can handle) and the situation got better, but it is still too slow. Another problem is that the SCU operates the actuators only at 50Hz, and that frequency is fixed. Hardware and firmware revision would be needed in order to increase the sampling/actuating frequency any further. Such upgrade is planned for the future, because at least 100Hz sampling/actuating frequency would be needed for a completely stable tail control. The summary of the yaw rate identification/control experiments and control loop settings can be found in section Yaw Rate Control.

The pitch and roll rates are generally much slower than the yaw rate dynamics. This is caused by several factors; The most significant is the large gyroscopic effect of the main rotor, which has strong damping effect on the pitch and roll motions. The Bell-Hiller stabilizer (if present) is another important factor, especially if the flybar paddles are heavy. The pitch and roll rate actuators (the longitudinal and lateral cyclic controls) are the same, but the vehicle response in pitch is somewhat slower than in the roll, because the moment of inertia of the fuselage is much greater for the pitch movements compared to the roll. The maximal pitch and roll rates are approximately the same (in the range of 80-150°/s for a typical small UAV), but the angular acceleration in pitch is slower and the vehicle inertia is greater, compared to the roll. Experience shows that around 25Hz sampling and actuating frequency is sufficient for the pitch and roll rate control. The RAMA system operates at the same frequency for each axis, so the pitch and roll control loops are running at the same 64Hz sampling / 50Hz actuating frequency as the yaw loop, which is more than enough for the purpose. The summary of the pitch and roll rate identification and control experiments and settings of the control loops can be found in sections Pitch Control, Roll Control and Roll and Pitch Identification Experiment.

Attitude Control Layer

Unfortunately, the ARCL is the only layer where no dependences among the control loops can be found; The situation becomes more complicated for the Attitude Control Layer (ACL). There are inherent dependencies among the attitude angle control between all three axes. For example, imagine a vehicle executing the following simple sequence of attitude maneuvers - first a 45° left roll, then 50° nose-up pitch and finally a 45° right roll. At the end of this sequence (assuming the original attitude was 0° yaw, 0° pitch and 0° roll, the vehicle will yaw 25° to the left, pitch 25° nose-up and roll by 0°.

Obviously, systematically correct solution would be some kind of MIMO (Multiple Input Multiple Output) controller, taking these dependences into account. However, if the vehicle tilts by reasonably small angle around a fixed working point, the dependences can be neglected and separate SISO controllers could be used for each axis. For each controller, the attitude change induced by other controllers into its axis would act merely as some additional disturbance. If the current attitude is used as a new working point in each sample, this approach will work for the entire attitude envelope. This solution works well under the assumption that the attitude maneuvers are reasonably slow, compared to the sampling frequency, which is a condition well fulfilled if the sampling frequency is sufficient for the angular rate control.

Setting new working point in each sample also solves another particularly annoying problem as by-product. This problem is a singularity point in the attitude measurement, occurring when an attitude angle changes from 359.9° to 0°. This change would trigger mayhem in the PID control loop if left unattended. If the working point for each attitude angle is set to say 180° in each sample (the setpoint must naturally be re-calculated with respect to the newly set working point), this condition becomes a non-issue.

The ACL layer in the RAMA system uses independent PID controllers in each axis. To simplify things even further, we use only a single fixed working point - residing at 0° yaw, 0° pitch and 0° roll. This is sufficient, since the tilt (roll and pitch) angles are limited to such extent that any dependencies among respective axes can be safely neglected.

The “singularity problem” for the 360° angle is not solved in the roll and pitch control loops, because we use internal angle representation ranging from -π radians to π radians, where -π (or π) angle would correspond to inverted flight. Since we assume that allowed roll and pitch angles would be well below that limit, there was no need to implement any solution to this problem.

This, of course, does not hold for yaw, since the yaw angle is unlimited. Therefore, we implemented very straightforward solution in the form of condition that if the yaw control error would be greater than π or smaller than -π, the measured yaw angle is locally converted into <0, 2π) or <0, -2π) range, so that the resulting absolute value of the control error after this conversion is smaller or equal to π. This conversion is done in each sampling point.

Position Control Layer

The Position Control Layer (PCL) works within latitude and longitude coordinates, represented in radians. There are two independent control loops; one for geographical latitude and the other for longitude. The difference between desired and measured latitude/longitude is used as the control error. The computed control actions must therefore be converted into desired longitudinal and lateral velocities of the vehicle (this is, effectively, a conversion from the earth frame into the body frame). The philosophy behind our architecture is that the yaw angle can be arbitrary and is not controlled by PCL; it could be set separately. The PCL computes desired longitudinal and lateral velocities according the current yaw angle (so the vehicle may flight forwards, sideways or backwards). This allows us to take benefit from the fact that a rotorcraft can fly in any direction regardless of its yaw angle.

This approach allows us to fully exploit unique characteristics of the rotorcraft flight envelope. It allows to perform maneuvers like hover with nose pointing in pre-set direction, flying sideways or backwards or tracking an arbitrary trajectory with nose always pointing in specific direction, which is very convenient for aerial photography.

The conversion of control actions is done according this formula:


where dlav and dlov are resulting latitude and longitude velocities, lata and lona are coputed latitude and longitude control actions, yaw_angle is the current yaw angle =) and max_vel is maximal allowed velocity.

In the current state of development, since the velocity layer is not present, max_vel is substituted by max_roll_angle and max_pitch_angle, which are, obviously, maximal allowed roll and pitch angles.

The computation of the latitude and longitude velocities is performed after each GPS sample, meaning that this control loop works at 1Hz period (since we have 1Hz GPS). Obviously, the yaw angle dynamics could be very fast and 1Hz sampling would not be sufficient for the velocity conversion (from the earth frame - determined by latitude and longitude - into the body frame, determining desired longitudinal and lateral velocities of the vehicle. Therefore, this conversion loop runs at 64Hz.

Trajectory Tracking Layer

The Trajectory Tracking Layer (TTL) is very simple. It reads and parses a text file, containing waypoints (set in relative coordinates) and desired yaw angle. The TTL sets a new waypoint and yaw angle for the Position Control Layer and waits until that waypoint is reached, then sets another waypoint. A timeout could be set, used to skip a waypoint if it is not reached within desired time.

The TTL runs at 1Hz period.

Altitude Control Layer

The Altitude Control Layer (ACL) is a very simple SISO (Single-Input Single-Output) control loop. The altitude is currently measured solely by the GPS, which severely constraints the control loop, since the measurement is not very accurate, can fluctuate quite a bit and is only available at 1Hz rate. All this highly compromises the control quality of this loop, but it is still robust enough to keep the vehicle in air :-), using very conservative controller settings. A P controller is sufficient for this task.

The controller output is applied directly to the collective and throttle servos. The throttle is controlled in open-loop; there is a simple relation between the collective setting and the throttle opening. The goal is to keep the revs of the rotor more or less constant, as was explained in chapter Flight Mechanics of a Rotorcraft. This method is fairly sufficient and widely used, since the engine is pretty self-stabilized because of its mechanically tuned muffler. It is possible to control the throttle in a separate feedback control loop, but that would require installing some rev sensor on the rotor, which is probably not worth the effort.

It is assumed that the vehicle is always flying upright in a near-level attitude, which is in turn ensured by roll and pitch angle controllers.

[1] Astrom, K. J. and Wittenmark, B. - Computer-Controlled Systems, 3rd edition, Prentice Hall, NJ, USA, 1997. ISBN 0-13-314899-8

control_algorithms.txt · Last modified: 23/10/2010 10:18 by ondra
Recent changes RSS feed Creative Commons License Powered by PHP Valid XHTML 1.0 Valid CSS Driven by DokuWiki