Figure 4.5: Illustration of the same side test.
By relating the corner coordinates of the GFV with the UAV’s position, together with the camera attitudes, we are able to calculate geo-referenced corner coordinates, assuming an approximately flat ground2. The geo-referenced frame’s center point is given by r
proj while the geo-referenced four corners of the image frame corresponding to front/rear and starboard/port of the UAV are given by rf s, rf p, rrs, rrp, respectively. By assuming these coordinates to be
2
30 4.2. Projected camera image constraints
known (see section 4.1), we can determine whether an object is within the GFV.
For an object to be within the square defined by the corners F P, F S, RP and RS, as shown in figure 4.5 above, the object must be to the left of the line through the points F S and RS, to the right of the line through F P and RP , above the line through RP and RS and below the line through F P and F S. If we calculate the cross product of the vectors [F S− F P ] and [P 1− F P ] we’ll get a vector pointing out of the paper plane. On the other hand, if we take the cross product of the vector [F S− F P ] and [P 2 − F P ] we’ll get a vector pointing into the paper plane. In fact, if we cross [F S− F P ] with the vector from F P to any point above the vector [F S− F P ], the resulting vector points out of the paper plane. Using any point below the vector [F S− F P ] yields a vector pointing into the paper plane. This means by using the cross product we are able to determine which side of a line (vector) a point is located. The only problem is to determine whether the resulting vector should point into the paper plane or out of the paper plane. This can be solved using a reference point. I.e. if we want to determine whether the point P 2 in figure 4.5 is below or above the vector formed by [F S− F P ] all we need to do is calculate the cross product [F S− F P ] × [P 2 − F P ] and compare the result with a reference, e.g. [F S− F P ] × [RP − F P ] or [F S − F P ] × [RS − F P ]. If the resulting vectors points in the same direction, the point P 2 is on the same side of [F S− F P ] as RP or RS. By taking the dot product of the two resulting vectors we are able to determine if they are pointing in the same direction, which should result in a positive solution to the dot product. We can summarize the constraints for the UAV’s GFV as
((F S− F P ) × (RP − F P )) · ((F S − F P ) × (robj− F P )) ≥ 0 ((RS− F S) × (RP − F S)) · ((RS − F S) × (robj− F S)) ≥ 0 ((RP− RS) × (F P − F S)) · ((RP − RS) × (robj− RS)) ≥ 0 ((F P − RP ) × (F S − RP )) · ((F P − RP ) × (robj− RP )) ≥ 0,
(4.9)
where robj is the coordinates of the object3. If the constraints above hold, the object given by robj is located within the GFV. Figure 4.6 summarizes the GFV’s constraints expressed with the image corners’ geo-referenced coordinates. As we can see, a red cross marks an object outside the field of view, while a blue cross marks an object within the field of view. We refer to Ericson (2004, ch. 5.4) for collision avoidance tests such as the method described in this section.
3
Note that all coordinates have to be given in the form [x, y, z] since the cross product needs vectors of 3 dimensions.
Chapter 4. Field of View 31
Chapter 5
Model Predictive Control
To achieve the goal of tracking objects by controlling an UAV equipped with a gimbal, we have decided to use a model predictive controller (MPC). By relating the UAV’s and gimbal’s attitudes to the camera, we are able to calculate the camera lens’ center projected down on earth together with the projected camera image’s corner coordinates. This was done in chapter 3 and 4. By using the same approach we can calculate the gimbal’s pan and tilt angles, α an β, which will place the object of interest in the center of the camera lens1.
Before proceeding with deriving equations for the gimbal’s attitude in section 5.2, we present some of the main MPC theory in section 5.1. The UAV’s attitude is defined in section 5.3 while the objective function used throughout this thesis is described in section 5.5. One should note that all coordinates and calculated variables in this chapter are discrete and time dependent for {t + k} ∀k ∈ [0, T ], where T is the finite time horizon. The subscript {t + k} is mostly omitted due to increase readability.
5.1 Background
The necessity in the process industry in the early 1970s to satisfy increasing production requirements like economic optimization, maximum exploitation of production capacities and minimum variability in product qualities required new control methods. A decade before, in the early 1960s, Rudolf E. Kalman started the development of modern control concepts, founded on the stability properties of linear control systems. Kalman’s objective was to determine when a linear control system was optimal. His study resulted in, among other concepts, a linear quadratic regulator (LQR) which was designed to minimize an unconstrained quadratic objective function of states and inputs. Due to infinite horizons, the LQR had great stability properties. The only problem was the LQR didn’t have any constraints describing the control objective’s physical behavior. Because of this the LQR had little impact on the control development in the process industries (Ruchika, 2013).
Kalman’s work led to the birth of the model predictive control method (MPC), where simplified models describing control objectives’ dynamics were included and calculated in a finite time horizon. By this, one could control plants based on future predictions calculated from simplified models. The MPC was first brought to the process industry in the late 1970s were it was used in chemical plants and oil industry like IDCOM (1978) and Shell (1979). Since then it has gained momentum in the process industries. MPCs are often used in a strategic layer (Foss and Heirung, 2013), above the control layer, called advanced process control. This layer provides set-points to the control layer in order to control the process. In addition to process industry there is ongoing research based on using MPCs in other fields. A few examples are
• power control (e.g. PMS in offshore operations),
1
I.e. center of the GFV.
34 5.1. Background
• trajectory tracking (e.g. welding robots),
• force allocations (e.g. thrust allocation in DP systems), • economy (e.g. investment analysis).
In short, MPC can be defined as (Mayne et al., 2000)
Model predictive control is a form of control in which the current control action is obtained by solving, at each sampling instant, a finite horizon open loop optimal control problem, using the current state of the plant as the initial state; the optimization yields an optimal control sequence and the first control in this sequence is applied to the plant.
Since the birth of the MPC in the 1970s, a variety of different algorithms have been developed to handle various aspects like discrete and continuous input signals and measurements, moving time horizons, constrained/unconstrained variables and blocking of control variables (manipulated variables (MV), (Hauger, 2013)). The similarity between the different MPC applications is that they all try to minimize an objective function (also called cost function) subjected to equality and inequality constraints based on well-known optimization methods like LSQ, QP and SQP. Since dynamic systems almost always include nonlinear elements, the nonlinear MPC (NMPC) is often used for prediction calculations based on system models. An example of an objective function is given below, both in the continuous and discrete case. One should note that a MPC’s objective would be to minimize or maximize one or more objective functions.
J(t) = 1 2 Z t t0 h z(τ )Q(τ )z⊤(τ ) + dz(τ )z(τ ) + u(τ )R(τ )u⊤(τ ) + du(τ )u(τ ) i dτ, (5.1a) Jt+k = 1 2 t+k−1 X i=t h zi+1Qi+1z⊤i+1+ dz,i+1zi+1+ uiRiu⊤i + du,iui i . (5.1b)
In addition, one often add the terms (here given in discrete terms) ∆uiR∆u∆u⊤i + d∆u,i∆ui, where ∆ui = ui− ui−1, to prevent rapid changes in the manipulated variables. Furthermore, the objective function could also consist of a Lagrange term and/or a Mayer term,
J(t) = ϑ (t, z(t), u(t)) + Z t t0 L (τ, z(τ ), u(τ )) dτ, (5.2a) Jt+k = ϑt+k(t + k, zt+k, ut+k) + t+k X i=t Lt+k(t + k, zt+k, ut+k) , (5.2b)
where ϑ (t, z(t), u(t)) represents an arbitrary Mayer term and L (τ, z(τ ), u(τ )) represents an arbitrary Lagrange term. In case of dynamic changing constraints subjected to the objective functions one should include slack variables to avoid an infeasible optimization problem. This is done when e.g. limits like control variable limits (umin ≤ u ≤ umax) changes. Let’s say the control u is at it’s maximum limit umax when umax decreases. u will now suddenly be out of bound and the optimization problem becomes infeasible. By introducing slack variables (umin−s ≤ u ≤ umax+s, ∀s ≥ 0) and add the term ρs+Rss⊤, where Rsand ρ are penalties,
Chapter 5. Model Predictive Control 35
to the objective function2the optimization problem would stay feasible even if the limits change.
A schematic illustration of the MPC’s structure is shown in figure 5.1 (Hauger, 2013). The dashed lines represents correction flows and interactions between the blocks. The prediction calculated by the model is used to update parameters in the controller, and the controller can, by adaptive methods, update parameters (parameter estimation) and do corrections due to simplifications in the model. The different colors represents calculation flows. The blue loop showcases the distribution of the controlled variables (CV), z, and the red loop represents the disturbance variables (DV), ν, which is introduced in the system and can be measured and corrected for. The brown loop showcases distribution of the manipulated variables (MV), u, while the purple loop measurement updates (both estimated/calculated and measured), y and ¯ y. Controller Process Measurements Estimator Model u (MVs) z (CVs) Predictions Setpoints Constraints − ¯ y Updated states and parameters ν (DVs) Modelled y measurements
Figure 5.1: MPC - Scheme (Hauger, 2013).
Before we address and discuss the MPC formulation any further we need to establish the relations between the gimbal’s tilt and pan angles and the projected image frame. This is done in section 5.2, while the UAV’s attitude is stated in section 5.3.