Norwegian University of Science and Technology

Helicopter Lab
TTK4135 Optimization and Control
Spring 2012

Group members

10039 10045 10056 10075 10088

Abstract
The purpose of this project is to utilize optimization and control theory, reducing cost functions with constraints, and control the helicopter both with and without feedback. The project is a mandatory assignment in the course TTK4135 Optimization and Control. Tasks throughout this report are mainly solved using MATLAB and Simulink. The solutions are tested at the helicopter lab and compared with its optimal trajectory. The following topics will be covered Discretization of the mathematical model. Optimal control of pitch/travel, with and without feedback. Optimal control of pitch/travel and elevation, with and without feedback. Appendices containing developed Simulink diagrams, plots and MATLAB les. We assume that the reader has basic knowledge regarding optimization and control theory.

Contents
1 2 The Mathematical Model and Nomenclature Optimal Control of Pitch/Travel without Feedback 2.1 Continuous time state space model . . . . . . . 2.2 Discrete state space model . . . . . . . . . . . . 2.3 Optimal Trajectory . . . . . . . . . . . . . . . . 2.4 Simulation using optimal input sequence . . . 1 2 2 2 3 4 5 5 5 5 7 7 7 8 8 9 9 10 11 11 15 16 19 19 22 25 28 31 31 32 32 33 33 34 34 34 35

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

Optimal Control of Pitch/Travel with Feedback 3.1 Linear Quadratic (LQ) Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Implementation of feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Model Predictive Control (MPC) . . . . . . . . . . . . . . . . . . . . . . . . . . . . Optimal Control of Pitch/Travel and Elevation with and without Feedback 4.1 Continuous state space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Discrete state space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Inequality constraints on elevation . . . . . . . . . . . . . . . . . . . . . . 4.4 Optimal input Sequence with and without feedback . . . . . . . . . . . . 4.5 Decoupling of states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.6 Optional exercise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

References A Plots 1.1 Task 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Task 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Task 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . B MATLAB Codes 2.1 Task 2.3 . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Task 2.4 . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Task 3.2 . . . . . . . . . . . . . . . . . . . . . . . . . 2.4 Task 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . 2.4.1 Objective function . . . . . . . . . . . . . . 2.4.2 Nonlinear constraints . . . . . . . . . . . . 2.5 Accompanying scripts . . . . . . . . . . . . . . . . 2.5.1 Initialization le for helicopter 3 . . . . . . A 2.5.2 Enable L TEX interpretation of plot legends 2.5.3 Automate creation of PDF plots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

C Simulink Diagrams 3.1 Simulink diagram for Task 2.4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Simulink diagram for Task 3.2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Simulink diagram for Task 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

III

List of Figures
3.1 A.1 A.2 A.3 A.4 A.5 A.6 A.7 A.8 C.1 C.2 C.3 Control hierarchy when using MPC (from assignment text). . . . . . . . . . . . . Optimal control value u and resulting states x using q = 0.1. . . . . . . . . . . Optimal control value u and resulting states x using q = 1. . . . . . . . . . . . Optimal control value u and resulting states x using q = 10. . . . . . . . . . . . Calculations compared to measurements when applying the optimal control value u = p directly without feedback. The set-points p are weighted by q = 1. c ci Calculations compared to measurements when using the optimal control law uk = u K ( xk x ), thus correction through feedback. . . . . . . . . . . . . . k k Optimal control without feedback. Nonlinear constraints applied to elevation. . Optimal control with feedback. Nonlinear constraints applied to elevation. . . . Optimal control with feedback and an extended control horizon, N = 80. Nonlinear constraints applied to elevation. . . . . . . . . . . . . . . . . . . . . . . . . . Simulink diagram for Task 2.4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulink diagram for Task 3.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Simulink diagram for Task 4.3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 11 12 13 14 15 16 17 18 34 34 35

IV

NTNU Department of Engineering Cybernetics

Helicopter lab

The Mathematical Model and Nomenclature

The mathematical model used in this assignment is summarized by the following equations e + K3 Ked e + K3 Kep e = K3 Kep ec p + K1 K pd p + K1 K pp p = K1 K pp pc =r r = K2 p Parameter values and descriptions are given in Tables 1.1 and 1.2. Table 1.1: Parameters and values Symbol la lh Kf Je Jt Jp mh mw mg Kp Parameter Distance from elevation axis to helicopter body Distance from pitch axis to motor Force constant motor Moment of inertia for elevation Moment of inertia for travel Moment of inertia for pitch Mass of helicopter Balance weight Effective mass of the helicopter Force to lift the helicopter from the ground Table 1.2: Variables Symbol p pc r rc e ec Vf Vb Vd Vs K pp , K pd , Kep , Kei , Ked Tg Variable Pitch Setpoint for pitch Travel Speed of travel Setpoint for speed of travel Elevation Setpoint for elevation Voltage, motor in front Voltage, motor in back Voltage difference, Vf Vb Voltage sum, Vf + Vb Controller gains Moment needed to keep the helicopter ying Value 0.63 0.18 0.25 0.83 0.83 0.034 1.05 1.87 0.05 0.49 Unit m m N/V kgm2 kgm2 kgm2 kg kg kg N (1.1a) (1.1b) (1.1c) (1.1d)

Page 1 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

Optimal Control of Pitch/Travel without Feedback

This part of the assignment covers optimal control of pitch and travel without using feedback. An optimal input sequence is computed as the solution of a QP problem, and applied to the helicopter. The sequence makes the helicopter travel an angle of 180 degrees.

2.1

Continuous time state space model

A general linear continuous time state space model can be written as x = Ac x + Bc u In this part the following state vector and controller output is used. x= r p p and u = pc (2.1)

By rearranging the equations of the summarized model (1.1), we obtain =r r = K2 p p = K1 K pd p K1 K pp p + K1 K pp pc which can be stated in the following state space representation 0 1 0 0 0 0 0 K2 0 x+ 0 u x= 0 0 0 0 1 0 0 K1 K pp K1 K pd K1 K pp
Ac Bc

(2.2a) (2.2b) (2.2c)

(2.3)

This model incorporates four states, consisting of travel, speed of travel, pitch and pitch rate. The input to the model is the desired pitch angle. As we see, no equations for the elevation or elevation rate are included in the model. This can be done because the basic control layer (Figure 7 in the assignment) takes care of the elevation, using a constant reference. The rest of the system can assume that the elevation is constant and independent of the rest of the dynamics. Decoupling the states in this way simplies the model and control problem, but the model accuracy will decrease.

2.2

Discrete state space model

The continuous state space model (2.3) is to be discretized using the forward Euler method. Eulers method approximates derivatives by lines of constant slope between consecutive points in time, mathematically written as x x k +1 x k t (2.4)

where t is the time step between each sample k. The continuous model (2.3) can be discretized as follows x k +1 x k = Ac xk + Bc uk t xk+1 = ( I + tAc ) xk + tBc uk (2.5)
A B

Page 2 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

Expanding the discrete-time matrices A and B for our system, yields 1 t 0 0 0 0 1 tK2 0 0 uk xk + = 0 0 1 t 0 0 0 tK1 K pp 1 K1 K pd tK1 K pp

x k +1

(2.6)

The state vector x and controller output u remains the same as depict in (2.1).

2.3

Optimal Trajectory

The following cost function is to be minimized =

i =1

( i f )

+ qp2 ci

where

q0

(2.7)

with the given linear constraints

| pk | <

30 180

and

| pck | <

30 180

for all

k {1, . . . , N }

(2.8)

which limits the pitch and pitch setpoint. The aim is to move the helicopter from the initial point x0 = 0 0 0 0 to the desired endpoint x f = f 0 0 0 , given 0 = and f = 0, i.e. a travel of 180 degrees. From (2.7) it is clear that this is a QP problem, which can be rewritten to the standard representation = 1 2
N 1 k =0

{ xk Qxk + uk Ruk }

(2.9)

Despite the unusual naming q, the weight matrices are identied as Q = 2 diag 1 0 0 0 and R = 2q (2.10)

Further transformation of the problem, to include the summation internally in the matrices, yields min z Gz
z

(2.11a) (2.11b)

s.t.

AQP z = BQP

where the new state vector, which includes all the time steps of both x and u, is given by z = x1 . . . x N , u0 . . . u N 1

R3001

(2.12)

Page 3 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

The rest of the matrices are constructed as follows Q 0 0 . . 0 ... ... . . .. . .. . . . Q . . 300300 . G=. .R .. .. . . . R . . . . .. .. . . . 0 . 0 I

(2.13)

AQP

BQP

0 B 0 0 . . .. .. .. . . A I . . . . 0 . . . . R240300 .. .. .. .. .. .. . . = 0 . . . . . . . . . . . . .. .. .. .. .. . . . . . 0 . . . . 0 0 0 A I 0 0 B Ax0 0 = . R2401 . . 0

(2.14)

(2.15)

The dimensions shown are for both a control horizon and a prediction horizon of N = M = 60 time steps. Now, the system is ready to be solved by MATLABs quadprog function. Throughout this task it is assumed a constant, decoupled, elevation angle. The term (i )2 is a calculated deviation after each indices, with regards to travel. Unwanted effects can arise when steering the helicopter to = f with this objective function, since it does not take the travel rate into account. A result can be that the helicopter just passes through f (without stopping) when the optimal sequence is nished. The presented plots in Figures A.1, A.2 and A.3, shows the computed optimal control values u and states x for q = 0.1, q = 1 and q = 10, respectively. A high value of q will make the pitch setpoint term dominate the cost function, thus high values will be punished. The control becomes smoother as well as reducing wear and tear on the machinery. A result of good pitch control is a smoother travel trajectory, but the duration of the optimal travel increases for high values of q and decreases for low values.

2.4

Simulation using optimal input sequence

Figure A.4 shows how the helicopter behaves when the calculated optimal input sequence u is implemented. The parameter q is set to 1, so the error term ( f )2 and the control-value qp2 are equally punished in the objective function (2.7). ci As seen in the plot, both travel and travel rate miss the desired endpoint of zero because of the lack of feedback. If we had a perfect model, no deviation would be observed. In real life there will always be model errors, some large enough to require feedback. The model error of course increases when discretizing.

Page 4 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

Optimal Control of Pitch/Travel with Feedback

In this task, feedback is introduced to the controller from the previous task.

3.1

Linear Quadratic (LQ) Controller

Feedback is introduced to the optimal controller (2.1) in form of a linear quadratic controller. This means that the controller minimizes a quadratic criteria J, given by J=

i =0

xi+1 QLQ xi+1 + ui

RLQ ui

(3.1)

where QLQ 0 and RLQ > 0 are diagonal matrices weighting the deviation x and u from the optimal pre-calculated values x and u . Feedback is introduced with the following manipulated variable uk = u K ( xk x ) k k (3.2)

The optimal feedback matrix K was calculated by the MATLAB function dlqr, assuming the manipulated variable is given by (3.2). Computation yields K = 1.3170 3.5785 0.3113 0.0737 when using the weight matrices QLQ = diag 10 0.2 0.1 0 and RLQ = 1 (3.4) (3.3)

The weights are mainly based on trial and error, with emphasis on the travel . The derivative variables r and p have low weights, because they contain high frequency noise, which is undesirable in this control system. The value of R makes u smoother, thus minimizing wear and tear on the helicopter.

3.2

Implementation of feedback

Figure A.5 shows the response of the helicopter compared to the computed values. In the appendix, Figure C.2 shows the Simulink diagram for the simulation. Feedback makes the system able to measure the helicopters position and pitch angle for each time step. When feedback is implemented into the model, the travel will follow the optimal trajectory better than without feedback. The pitch however, will follow the optimal trajectory better without feedback. Since travel is related to pitch, and the travel is weighted higher, the helicopter will follow the travel trajectory. It is not possible to make both pitch and travel follow the optimal trajectories perfectly at the same time.

3.3

Model Predictive Control (MPC)

Using Model Predictive Control, the control hierarchy would be similar to Figure 3.1. In this system, the Product costs will be the weight on each of the setpoints in the objective function. A high weight indicates a high cost (e.g. expensive fuel, maintenances costs, etc.), which should be avoided. With MPC there is a moving prediction horizon, this should be chosen so that it includes the bulk part of the reference, this will determine the length of the prediction horizon. Model

Page 5 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

Product costs Plant constraints

Model-based optimization with Model PredictiveContol (MPC)

u*

x*

Linear-Quadratic Regulator (LQR)

u
Pitch controller (PD) Elevation controller (PID)

Vd, Vs
Plant (helicopter)

Figure 3.1: Control hierarchy when using MPC (from assignment text). accuracy depends on the re-optimization, which is executed after each time step in which errors are corrected. Re-optimizations means that for each step there is a new optimization problem being solved, for instance LP problem, QP problem, etc. The main objective with MPC control is to calculate future variables in order to optimize future behavior. This is accomplished by tuning the weight matrices Q and R as well as adjusting the length of the prediction horizon. Tuning is done in order to obtain an acceptable closed-loop performance. Shorter horizon gives lower complexity optimization problems at each time step, which result in less computational load. A longer horizon gives better performance, but increases computational load at each time step. The following is a summary of the properties of LQR and MPC (Imsland 2007). Linear Quadratic Regulator Advantages It is inherently multivariable (it takes care of couplings in the process). It is optimal on the innite horizon. It can be shown that it has good robustness properties (due to feedback). Disadvantages It does not look to the future. It does not handle constraints on states and inputs. Model Predictive Control Advantages It looks to the future. It handles constraints on states and inputs. Disadvantages It is not optimal. It requires more computational power, and have strict real-time demands.

Page 6 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

Optimal Control of Pitch/Travel and Elevation with and without Feedback

The objective of this task is optimal control of pitch/travel and elevation with and without feedback; that is, we have an optimal trajectory in two dimensions.

4.1

Continuous state space

Introducing two additional states, elevation e and elevation rate e, the new state vector becomes x = r p p e e , u = pc ec (4.1) Writing the system on continuous state space form, yields 0 0 0 x= 0 0 0 1 0 0 0 0 0 K2 0 0 0 0 0 1 0 0 x 0 K1 K pp K1 K pd 0 0 0 0 0 0 1 0 0 0 K3 Kep K3 Ked 0 0 0 0 0 0 u + 0 K1 K pp 0 0 0 K3 Kep

(4.2)

4.2

Discrete state space

The acquired state space model is discretized using the forward Euler method. This was solved in the same manner as in section (2.2). Thus resulting in 1 t 0 0 0 0 0 1 tK2 0 0 0 0 0 1 t 0 0 xk = 0 0 tK1 K pp 1 tK1 K pd 0 0 0 0 0 0 1 t 0 0 0 0 tK3 Kep 1 tK3 Ked 0 0 0 0 0 0 uk + tK1 K pp 0 0 0 0 tK3 Kep

x k +1

(4.3)

Page 7 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

4.3

Inequality constraints on elevation

The objective function that is to be minimized, is stated as =

i =1

( i f )

+ q1 pci 2 + q2 eci 2

(4.4)

The inequality constraint imposed on the elevation is given as ek e (k t ) where = 0.2, = 20 and t = 2 3 (4.6)
2

for all

k {1, . . . , N }

(4.5)

The MATLAB function fmincon was used in order to solve the optimization problem. This differs from the function used previously, since the problem at hand involves a nonlinear constraint instead of a linear constraint. After each time step the constraint (4.5) was implemented and passed to the function, in the form of c( xk ) = e (k t ) ek 0
2

(4.7)

The optimization problem was solved with different values for q1 and q2 . Initial values was q1 = q2 = 1. If the values are increased, large values of pci and eci are punished, resulting in fmincon failing to nd an optimal solution. This is intuitively right, since the system has less degrees of freedom when applying stricter constrains. The fmincon was not able to perform the optimization for N = 40. To get a better result a longer optimization and prediction horizon was tried. When N = 80, fmincon was able to calculate the optimal solution for the problem.

4.4

Optimal input Sequence with and without feedback

As in 3.1, we use a LQ controller to correct the optimal input sequence. It was necessary to re-tune the controller, and the best results were obtained when using QLQ = diag 5 0 0.1 0 2 0 and RLQ = diag 1 1 (4.8)

As the values indicates, the travel is assumed to be the most important state to control, and the pitch p thereafter. The resulting gain matrix became K=

0.9502 2.9437 0.3441 0.0692 0 0 0 0 0 0 0.6425 0.8635

(4.9)

Figure A.6 shows how the helicopter responds to the optimal sequence u when not using feedback. This causes the real pitch to deviate from the optimal pitch and real elevation to deviate from the optimal elevation. When feedback is introduced, as shown in Figure A.7, the LQ controller correct most of the earlier observed deviation in pitch and travel. Introducing feedback clearly improves the performance.

Page 8 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

The travel plots in both Figure A.6 (no feedback) and Figure A.7 (with feedback ) shows that the optimal travels are not brought to zero in an optimal way using M = N = 40. The algorithm is not able to calculate the optimal travel because the time horizon is to short and the constrains are to strict. Despite this, the travels are (eventually) brought to zero after the optimal sequence is nished, by the LQ controller. This is caused by the padding of zeros. Extending the control horizon and prediction horizon to M = N = 80 (as described in the previous task) gives the helicopter some extra time to stabilize. The results of this ight with feedback are shown in Figure A.8. It is clear that this work much better, but the optimization algorithm needs longer time to nd the solution.

4.5

Decoupling of states

In the model the rst 4 states are completely decoupled from the last 2. This is not the case in reality. In reality a change of the pitch would affect the elevation of the helicopter. Also the travel rate would be affected if a change in the elevation is made while the pitch angle is different from zero. An example of this is the observed behavior from the previous task: The elevation plots in Figure A.6 and Figure A.7 shows that the real elevation deviates from the optimal elevation. This happens because the last two states are decoupled from the rst four in the model of which the optimal trajectory is based. This makes the elevation change of the helicopter occur earlier than the change of the optimal elevation, because the helicopter speeds up when it tries to increase the elevation. A solution to this would be to improve the model, ensuring coupling between all states. This also implies possibly making the model non-linear (which fmincon can handle).

4.6

Optional exercise

We tried to add constraints to the last state vector, x N = 0, to force all the states to zero at the end of the horizon. It was clear that the problem then became infeasible. Attempting to make the problem feasible, we relaxed some of the states, but with no luck.

Page 9 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

References
Foss, B. A. (2004), Linear quadratic control, Rev. 2007 . Imsland, L. (2007), Introduction to model predictive control. Knuth, D. E., Larrabee, T. & Roberts, P. M. (1997), Mathematical Writing, Knowl. Eng. Rev. 12, 331334. URL: http://dl.acm.org/citation.cfm?id=976246.976258 Nocedal, J. & Wright, S. (2006), Numerical optimization, Springer series in operations research, 2 edn, Springer.

Page 10 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

A
1.1

Plots
Task 2
40 Optimal control value u = p c Degrees [deg] 20 0 20 40 200 Optimal travel 150 Degrees [deg] 100 50 0 50 10 Optimal travel rate r Degrees/sec [deg/s] 0 10 20 30 40 Optimal pitch p Degrees [deg] 20 0 20 40 200 Optimal pitch rate p Degrees/sec [deg/s] 100 0 100 200 300 0 5 10 Time [s] 15 20 25

Figure A.1: Optimal control value u and resulting states x using q = 0.1.

Page 11 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

40 Optimal control value u = p c Degrees [deg] 20 0 20 40 200 Optimal travel 150 Degrees [deg] 100 50 0 50 10 Optimal travel rate r Degrees/sec [deg/s] 0 10 20 30 40 Optimal pitch p Degrees [deg] 20 0 20 40 200 Optimal pitch rate p Degrees/sec [deg/s] 100 0 100 200 300 0 5 10 Time [s] 15 20 25

Figure A.2: Optimal control value u and resulting states x using q = 1.

Page 12 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

30 Optimal control value u = p c Degrees [deg] 20 10 0 10 200 Optimal travel 150 Degrees [deg] 100 50 0 50 5 Optimal travel rate r Degrees/sec [deg/s] 0 5 10 15 20 40 Optimal pitch p Degrees [deg] 20 0 20 40 200 Optimal pitch rate p Degrees/sec [deg/s] 100 0 100 200 300 0 5 10 Time [s] 15 20 25

Figure A.3: Optimal control value u and resulting states x using q = 10.

Page 13 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

40 Optimal control value u = p c Degrees [deg] 20 0 20 40 200 Optimal travel Real travel Degrees [deg] Degrees/sec [deg/s] 100

100 20 0 20 40 Optimal travel rate r Real travel rate r 60 40 Optimal pitch p Real pitch p Degrees [deg] 20 0 20 40 150 Degrees/sec [deg/s] Optimal pitch rate p Real pitch rate p 100 50 0 50

10 Time [s]

15

20

25

Figure A.4: Calculations compared to measurements when applying the optimal control value u = p directly without feedback. The set-points p are weighted by q = 1. c ci

Page 14 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

1.2

Task 3
40 Optimal control value u = p c Degrees [deg] 20 0 20 40 200 Optimal travel Real travel Degrees [deg] Degrees/sec [deg/s] 150 100 50 0 10 0 10 20 Optimal travel rate r Real travel rate r 30 40 Optimal pitch p Real pitch p Degrees [deg] 20 0 20 40 150 Degrees/sec [deg/s] Optimal pitch rate p Real pitch rate p 100 50 0 50

10 Time [s]

15

20

25

Figure A.5: Calculations compared to measurements when using the optimal control law uk = u K ( xk x ), thus correction through feedback. k k

Page 15 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

1.3

Task 4
40 Degrees 20 0 Optimal pitch control u = p c 1 20 20 Degrees 10 0 Optimal elevation control u = e c 2 10 200 Degrees

0 Optimal travel Real travel 200 20

Degrees/sec

0 20 40 50 Optimal travel rate r Real travel rate r

Degrees

0 Optimal pitch p Real pitch p

50 200 Degrees/sec

0 Optimal pitch rate p Real pitch rate p 200 15 Optimal elevation e Real elevation e

Degrees Degrees/sec

10 5 0 5 20

0 Real elevation rate e Optimal elevation rate e 20 0 2 4 6 8 10 Time [s] 12 14 16 18 20

Figure A.6: Optimal control without feedback. Nonlinear constraints applied to elevation.

Page 16 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

40 Degrees 20 0 Optimal pitch control u = p c 1 20 20 Degrees 10 0 Optimal elevation control u = e c 2 10 200 Degrees

0 Optimal travel Real travel 200 20

Degrees/sec

0 20 40 50 Optimal travel rate r Real travel rate r

Degrees

0 Optimal pitch p Real pitch p

50 200 Degrees/sec

0 Optimal pitch rate p Real pitch rate p 200 15 Optimal elevation e Real elevation e

Degrees Degrees/sec

10 5 0 5 20

0 Real elevation rate e Optimal elevation rate e 20 0 2 4 6 8 10 Time [s] 12 14 16 18 20

Figure A.7: Optimal control with feedback. Nonlinear constraints applied to elevation.

Page 17 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

50 Degrees

0 Optimal pitch control u = p c 1 50 20

Degrees

10 0 Optimal elevation control u = e c 2 10 200

Degrees

0 Optimal travel Real travel 200 20

Degrees/sec

0 20 40 50 Optimal travel rate r Real travel rate r

Degrees

0 Optimal pitch p Real pitch p

50 200 Degrees/sec

0 Optimal pitch rate p Real pitch rate p 200 15 Optimal elevation e Real elevation e

Degrees Degrees/sec

10 5 0 5 20

0 Real elevation rate e Optimal elevation rate e 20 0 5 10 15 Time [s] 20 25 30

Figure A.8: Optimal control with feedback and an extended control horizon, N = 80. Nonlinear constraints applied to elevation.

Page 18 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

B
2.1
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66

MATLAB Codes
Task 2.3

% oppg10_2_3.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%% 10.2.3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all for runde = 1:1:3 % qloop disp(['RundeXXX ', sprintf('%u', runde)]); close all init; delta_t = 0.25; sek_forst = 5;

% tastetid

% Setter opp modellen for systemet. x = [lambda r p p_dot]' A1 = [1 delta_t 0 0; 0 1 delta_t*K_2 0; 0 0 1 delta_t; 0 0 delta_t*K_1*K_pp 1delta_t*K_1*K_pd]; B1 = [0; 0; 0; delta_t*K_1*K_pp]; % Antall pdrag og tilstander mx = size(A1,1); mu = size(B1,2); % Initialverdier lambda_0 = pi; x0 = [lambda_0 0 0 0]'; % Tidshorisont og initialisering N = 60; M = N; z = zeros(N*mx+M*mu,1); z0 = z; z0(1,1) = lambda_0; % Cost function I_n = eye(N); Qk = 2*diag([1 0 0 0]); Q = kron(I_n, Qk); % q er vekten p p^2_{ci}, i=1,...,N = ALLE pdrag. if runde == 1 q = 0.1; elseif runde == 2 q = 1; elseif runde == 3 q = 10; end

% Antall tilstander (antall rader i A) % Antall paadrag (antall kolonner i B)

% Initialverdier

% % % %

Tidshorisont for tilstander Tidshorisont for pdrag Initialiserer z for hele horisonten Startverdi for optimaliseringen

Rk = 2*q; R = kron(I_n, Rk); G = blkdiag(Q, R);

% Equality constraint I_n = eye(N); Aeq_c1 = eye(N*mx); Aeq_c2 = kron(diag(ones(N1,1),1), A1);

% Component 1 of A % Component 2 of A

Page 19 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139

Aeq_c3 = kron(I_n, B1); Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3]; beq = [A1*x0; zeros((N1)*mx,1)];

% Component 3 of A

%%%%%%%%%%%%%% %%%% Krav %%%% %%%%%%%%%%%%%% % % p_k < 30 pi / % OG % p_c < 30 pi / % % Husk at p_k = % p_c = % %%%%%%%%%%%%%%

180 180

= tilstanden x3_k = pdraget u_k

pitch = x3 og pdrag = u

p_limit = 30*pi/180; % degrees

% Inequality constraints ul = p_limit*ones(N*mu,1); % 30 deg < pitchpdrag < 30 deg uu = p_limit*ones(N*mu,1); ul(N*mu) = 0; % Siste pdrag skal vre 0 uu(N*mu) = 0; xl = Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning) xu = Inf(N*mx,1); xl(3:mx:N*mx) = p_limit; % 30 deg < pitch x3 < 30 deg xu(3:mx:N*mx) = p_limit; lambda_f = 0; xf = zeros(mx,1); xf(1,1) = lambda_f; xl(N*mxmx+1:N*mx) = xf; xu(N*mxmx+1:N*mx) = xf; zl = [xl;ul]; zu = [xu;uu]; % Siste x_N = xf = [0 0 0 0]

% Solving the equality and inequalityconstrained QP with quadprog opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off'); tic [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt); t1 = toc;

% Beregnede paadrag og tilstander u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; x1 = [x0(1);z(1:mx:N*mx)]; x2 = [x0(2);z(2:mx:N*mx)]; x3 = [x0(3);z(3:mx:N*mx)]; x4 = [x0(4);z(4:mx:N*mx)]; Antall = sek_forst/delta_t; Nuller = zeros(Antall,1); Enere = ones(Antall,1); u x1 x2 x3 x4 = = = = = [Nuller; u; Nuller]; [lambda_0*Enere; x1; Nuller]; [Nuller; x2; Nuller]; [Nuller; x3; Nuller]; [Nuller; x4; Nuller];

% % % % %

Beregnet Beregnet Beregnet Beregnet Beregnet

paadrag tilstand tilstand tilstand tilstand

x1 x2 x3 x4

% figur t = (0:delta_t:delta_t*(length(u)1))';

% Virkelig tid

R2D = 180/pi; u = u*R2D; x1 = x1*R2D;

Page 20 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198

x2 = x2*R2D; x3 = x3*R2D; x4 = x4*R2D;

%%%%%%%%%%%%%%%%% % Lag figurer for q = 0.1, 1, 10 %%%%%%%%%%%%%%%%% figure(1) subplot(5,1,1) stairs(t,u) legend('Optimal control value $u^* = p_c^*$') ylabel('Degrees [deg]') fixplot; subplot(5,1,2) plot( t,x1,'mo'); legend('Optimal travel $\lambda^*$','Real travel $\lambda$') ylabel('Degrees [deg]') fixplot; subplot(5,1,3) plot(t,x2','mo'); legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast') ylabel('Degrees/sec [deg/s]') fixplot; subplot(5,1,4) plot(t,x3,'mo'); legend('Optimal pitch $p^*$','Real pitch $p$') ylabel('Degrees [deg]') fixplot; subplot(5,1,5) plot(t,x4','mo'); legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$') xlabel('Time [s]') ylabel('Degrees/sec [deg/s]') fixplot; set(gca,'XTickLabelMode','Auto');

set(gcf,'PaperType', 'A4') set(gcf,'PaperOrientation', 'portrait') papersize=get(gcf,'PaperSize'); set(gcf,'PaperPositionMode','manual') set(gcf,'PaperPosition',[0.25 0.25 papersize(1)0.5 papersize(2)0.5]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% filnavn = ['10_2_3_q' , sprintf('%u',q*10)]; createPdf(filnavn) grid on set(gcf,'Position',[100,00 700,700]) end % for

Page 21 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

2.2
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69

Task 2.4

% oppg10_2_4.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%% 10.2.4 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all init; delta_t = 0.25; sek_forst = 5;

% tastetid

% Setter opp modellen for systemet. x = [lambda r p p_dot]' A1 = [1 delta_t 0 0; 0 1 delta_t*K_2 0; 0 0 1 delta_t; 0 0 delta_t*K_1*K_pp 1delta_t*K_1*K_pd]; B1 = [0; 0; 0; delta_t*K_1*K_pp]; % Antall pdrag og tilstander mx = size(A1,1); mu = size(B1,2); % Initialverdier lambda_0 = pi; x0 = [lambda_0 0 0 0]'; % Tidshorisont og initialisering N = 60; M = N; z = zeros(N*mx+M*mu,1); z0 = z; z0(1,1) = lambda_0; % Cost function I_n = eye(N); Qk = 2*diag([1 0 0 0]); Q = kron(I_n, Qk); % q er vekten p p^2_{ci}, i=1,...,N = ALLE pdrag. q = 1;

% Antall tilstander (antall rader i A) % Antall paadrag (antall kolonner i B)

% Initialverdier

% % % %

Tidshorisont for tilstander Tidshorisont for pdrag Initialiserer z for hele horisonten Startverdi for optimaliseringen

Rk = 2*q; R = kron(I_n, Rk); G = blkdiag(Q, R);

% Equality constraint I_n = eye(N); Aeq_c1 = eye(N*mx); Aeq_c2 = kron(diag(ones(N1,1),1), A1); Aeq_c3 = kron(I_n, B1); Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3]; beq = [A1*x0; zeros((N1)*mx,1)];

% Component 1 of A % Component 2 of A % Component 3 of A

%%%%%%%%%%%%%% %%%% Krav %%%% %%%%%%%%%%%%%% % % p_k < 30 pi / 180 = tilstanden x3_k % OG % p_c < 30 pi / 180 = pdraget u_k % % Husk at p_k = pitch = x3 og

Page 22 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142

% p_c = pdrag = u % %%%%%%%%%%%%%%

p_limit = 30*pi/180; % degrees

% Inequality constraints ul = p_limit*ones(N*mu,1); % 30 deg < pitchpdrag < 30 deg uu = p_limit*ones(N*mu,1); ul(N*mu) = 0; % Siste pdrag skal vre 0 uu(N*mu) = 0; xl = Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning) xu = Inf(N*mx,1); xl(3:mx:N*mx) = p_limit; % 30 deg < pitch x3 < 30 deg xu(3:mx:N*mx) = p_limit; lambda_f = 0; xf = zeros(mx,1); xf(1,1) = lambda_f; xl(N*mxmx+1:N*mx) = xf; xu(N*mxmx+1:N*mx) = xf; zl = [xl;ul]; zu = [xu;uu]; % Siste x_N = xf = [0 0 0 0]

% Solving the equality and inequalityconstrained QP with quadprog opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off'); tic [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt); t1 = toc; % Beregnede paadrag og tilstander u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; x1 = [x0(1);z(1:mx:N*mx)]; x2 = [x0(2);z(2:mx:N*mx)]; x3 = [x0(3);z(3:mx:N*mx)]; x4 = [x0(4);z(4:mx:N*mx)]; Antall = sek_forst/delta_t; Nuller = zeros(Antall,1); Enere = ones(Antall,1); u x1 x2 x3 x4 = = = = = [Nuller; u; Nuller]; [lambda_0*Enere; x1; Nuller]; [Nuller; x2; Nuller]; [Nuller; x3; Nuller]; [Nuller; x4; Nuller];

% % % % %

Beregnet Beregnet Beregnet Beregnet Beregnet

paadrag tilstand tilstand tilstand tilstand

x1 x2 x3 x4

% figur t = (0:delta_t:delta_t*(length(u)1))'; tstop= 25; u_sim = [t,u]; %%%%%%%%%%%%%%%%% %sim heli_2_4 %%%%%%%%%%%%%%%%%

% Virkelig tid

R2D = 180/pi; u = u*R2D; x1 = x1*R2D; x2 = x2*R2D; x3 = x3*R2D; x4 = x4*R2D;

%%%%%%%%%%%%%%%%% load heli_2_4 %%%%%%%%%%%%%%%%%

Page 23 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188

figure(1) subplot(5,1,1) stairs(t,u) legend('Optimal control value $u^* = p_c^*$') ylabel('Degrees [deg]') fixplot; subplot(5,1,2) plot( t,x1,'mo', rt_lambda(:,1),rt_lambda(:,2)); legend('Optimal travel $\lambda^*$','Real travel $\lambda$') ylabel('Degrees [deg]') fixplot; subplot(5,1,3) plot(t,x2','mo', rt_r(:,1),rt_r(:,2)); legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast') ylabel('Degrees/sec [deg/s]') fixplot; subplot(5,1,4) plot(t,x3,'mo', rt_p(:,1),rt_p(:,2)); legend('Optimal pitch $p^*$','Real pitch $p$') ylabel('Degrees [deg]') fixplot; subplot(5,1,5) plot(t,x4','mo', rt_p_dot(:,1),rt_p_dot(:,2)); legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$') xlabel('Time [s]') ylabel('Degrees/sec [deg/s]') fixplot; set(gca,'XTickLabelMode','Auto');

set(gcf,'PaperType', 'A4') set(gcf,'PaperOrientation', 'portrait') papersize=get(gcf,'PaperSize'); set(gcf,'PaperPositionMode','manual') set(gcf,'PaperPosition',[0.25 0.25 papersize(1)0.5 papersize(2)0.5]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% createPdf('10_2_4_x') grid on set(gcf,'Position',[100,00 700,700])

Page 24 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

2.3
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69

Task 3.2

% oppg10_3_2.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% 10.3.1 + 10.3.2 %%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% init; delta_t = 0.25; sek_forst = 5;

% tastetid

% Setter opp modellen for systemet. x = [lambda r p p_dot]' A1 = [1 delta_t 0 0; 0 1 delta_t*K_2 0; 0 0 1 delta_t; 0 0 delta_t*K_1*K_pp 1delta_t*K_1*K_pd]; B1 = [0; 0; 0; delta_t*K_1*K_pp]; % Antall pdrag og tilstander mx = size(A1,1); mu = size(B1,2); % Initialverdier lambda_0 = pi; x0 = [lambda_0 0 0 0]'; % Tidshorisont og initialisering N = 60; M = N; z = zeros(N*mx+M*mu,1); z0 = z; % Cost function I_n = eye(N); Qk = 2*diag([1, 0, 0, 0]); Q = kron(I_n, Qk); q = 1; Rk = 2*q; R = kron(I_n, Rk); G = blkdiag(Q, R);

% Antall tilstander (antall rader i A) % Antall paadrag (antall kolonner i B)

% Initialverdier

% % % %

Tidshorisont for tilstander Tidshorisont for pdrag Initialiserer z for hele horisonten Startverdi for optimaliseringen

% Equality constraint I_n = eye(N); Aeq_c1 = eye(N*mx); Aeq_c2 = kron(diag(ones(N1,1),1), A1); Aeq_c3 = kron(I_n, B1); Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3]; beq = [A1*x0; zeros((N1)*mx,1)];

% Component 1 of A % Component 2 of A % Component 3 of A

p_limit = 30*pi/180; % degrees % Inequality constraints ul = p_limit*ones(N*mu,1); uu = p_limit*ones(N*mu,1); ul(N*mu) = 0; uu(N*mu) = 0; xl = Inf(N*mx,1); xu = Inf(N*mx,1); xl(3:mx:N*mx) = p_limit; xu(3:mx:N*mx) = p_limit; lambda_f = 0; xf = zeros(mx,1); xf(1,1) = lambda_f; % 30 deg < pitchpdrag < 30 deg % Siste pdrag skal vre 0 % Begrensning for tilstandene (ingen begrensning) % 30 deg < pitch x3 < 30 deg

Page 25 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142

xl(N*mxmx+1:N*mx) = xf; xu(N*mxmx+1:N*mx) = xf; zl = [xl;ul]; zu = [xu;uu];

% Siste x_N = xf = [0 0 0 0]

% Solving the equality and inequalityconstrained QP with quadprog opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off'); tic [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt); t1 = toc; % Optimal feedback matrix K, task 10.3.1 Qlq = diag([10 0.2 0.1 0]); Rlq = 1 ; [K,~,~] = dlqr(A1,B1,Qlq,Rlq);

% Beregnede paadrag og tilstander u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand Antall = sek_forst/delta_t; Nuller = zeros(Antall,1); Enere = ones(Antall,1);

x1 x2 x3 x4

u x1 x2 x3 x4

= = = = =

[Nuller; u; Nuller]; [lambda_0*Enere; x1; Nuller]; [Nuller; x2; Nuller]; [Nuller; x3; Nuller]; [Nuller; x4; Nuller];

% figur t = (0:delta_t:delta_t*(length(u)1))'; tstop = N*delta_t + 2*sek_forst; u_sim = [t,u]; x_sim = [t,x1,x2,x3,x4]; %%%%%%%%%%%%%%%%% %sim heli_3_2 %%%%%%%%%%%%%%%%%

% Virkelig tid

R2D = 180/pi; u = u*R2D; x1 = x1*R2D; x2 = x2*R2D; x3 = x3*R2D; x4 = x4*R2D; %% %%%%%%%%%%%%%%%%% load heli_3_2 %%%%%%%%%%%%%%%%% figure(1) subplot(5,1,1) stairs(t,u) legend('Optimal control value $u^* = p_c^*$') ylabel('Degrees [deg]') fixplot; subplot(5,1,2) plot( t,x1,'mo', rt_lambda(:,1),rt_lambda(:,2)); legend('Optimal travel $\lambda^*$','Real travel $\lambda$') ylabel('Degrees [deg]') fixplot;

Page 26 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178

ylim([0 200]); subplot(5,1,3) plot(t,x2','mo', rt_r(:,1),rt_r(:,2)); legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast') ylabel('Degrees/sec [deg/s]') fixplot; subplot(5,1,4) plot(t,x3,'mo', rt_p(:,1),rt_p(:,2)); legend('Optimal pitch $p^*$','Real pitch $p$') ylabel('Degrees [deg]') fixplot; subplot(5,1,5) plot(t,x4','mo', rt_p_dot(:,1),rt_p_dot(:,2)); legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$') xlabel('Time [s]') ylabel('Degrees/sec [deg/s]') fixplot; set(gca,'XTickLabelMode','Auto');

set(gcf,'PaperType', 'A4') set(gcf,'PaperOrientation', 'portrait') papersize=get(gcf,'PaperSize'); set(gcf,'PaperPositionMode','manual') set(gcf,'PaperPosition',[0.25 0.25 papersize(1)0.5 papersize(2)0.5]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% createPdf('10_3_2_feedback') grid on set(gcf,'Position',[100,00 700,700])

Page 27 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

2.4
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69

Task 4.3

% oppg10_4_3.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%% 10.4.3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

init; delta_t = 0.25; sek_forst = 5; global N M G mu mx alpha = 0.2; beta = 20; lambda_f = 2*pi/3;

% tastetid

beta alpha lambda_f q1 q2

% Setter opp modellen for systemet. x = [lambda r p p_dot]' A1 = zeros(6,6); A1(1,1) = 1; A1(1,2) = delta_t; A1(2,2) = 1; A1(2,3) = delta_t*K_2; A1(3,3) = 1; A1(3,4) = delta_t; A1(4,3) = delta_t*K_1*K_pp; A1(4,4) = 1delta_t*K_1*K_pd;%%%%%%%%%%%%%%%%%%%%%%%%%FEIL med minus 1 forst????????? A1(5,5) = 1; A1(5,6) = delta_t; A1(6,5) = delta_t*K_3*K_ep; A1(6,6) = 1delta_t*K_3*K_ed; B1 = zeros(6,2); B1(4,1) = delta_t*K_1*K_pp; B1(6,2) = delta_t*K_3*K_ep; % Antall pdrag og tilstander mx = size(A1,1); mu = size(B1,2); % Initialverdier lambda_0 = pi; x0 = zeros(mx,1); x0(1,1) = lambda_0;

% Antall tilstander (antall rader i A) % Antall paadrag (antall kolonner i B)

% Initialverdier

% Tidshorisont og initialisering N = 80; M = N; z = zeros(N*mx+M*mu,1); z0 = z; % N=80 tar 3.17 min % Cost function I_n = eye(N); Qk = diag([1 0 0 0 0 0]); Q = kron(I_n, Qk); q1 = 1; q2 = 1; Rk = diag([q1, q2]); R = kron(I_n, Rk); G = blkdiag(Q, R);

% % % %

Tidshorisont for tilstander Tidshorisont for pdrag Initialiserer z for hele horisonten Startverdi for optimaliseringen

% Equality constraint I_n = eye(N); Aeq_c1 = eye(N*mx);

% Component 1 of A

Page 28 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142

Aeq_c2 = kron(diag(ones(N1,1),1), A1); Aeq_c3 = kron(I_n, B1); Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3]; beq = [A1*x0; zeros((N1)*mx,1)];

% Component 2 of A % Component 3 of A

p_limit = 30*pi/180; % degrees xf = zeros(mx,1); xf(1,1) = 0; %lambda_f; % NBNBNBNBNB % Inequality constraints ul = Inf(N*mu,1); uu = Inf(N*mu,1); ul(1:mu:N*mu) = p_limit; % 30 deg < pitchpdrag < 30 deg uu(1:mu:N*mu) = p_limit; ul(N*mu) = 0; % Siste pdrag skal vre 0 uu(N*mu) = 0; ul(N*mu1) = 0; % Siste pdrag skal vre 0 uu(N*mu1) = 0; xl = Inf(N*mx,1); xu = Inf(N*mx,1); xl(3:mx:N*mx) = p_limit; xu(3:mx:N*mx) = p_limit; zl = [xl;ul]; zu = [xu;uu]; % Begrensning for tilstandene (ingen begrensning) % 30 deg < pitch x3 < 30 deg

% Solving with fmincon opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off'); tic [z,fval] = fmincon(@objective,z0,[],[],Aeq,beq,zl,zu,@constraint); %[z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt); t1 = toc; Qlq = diag([5 0 0.1 0 2 0]); Rlq = diag([1 1]); [K,~,~] = dlqr(A1,B1,Qlq,Rlq);

% Beregnede paadrag og tilstander u1 = [z(N*mx+1:mu:N*mx+M*mu);z(N*mx+M*mu1)]; % Beregnet paadrag u1 u2 = [z(N*mx+2:mu:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag u2 x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x1 x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x2 x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x3 x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand x4 x5 = [x0(5);z(5:mx:N*mx)]; % Beregnet tilstand x5 x6 = [x0(6);z(6:mx:N*mx)]; % Beregnet tilstand x6 Antall = sek_forst/delta_t; Nuller = zeros(Antall,1); Enere = ones(Antall,1);

u1 u2 x1 x2 x3 x4 x5 x6

= = = = = = = =

[Nuller; u1; Nuller]; [Nuller; u2; Nuller]; [lambda_0*Enere; x1; 0*Enere]; [Nuller; x2; Nuller]; [Nuller; x3; Nuller]; [Nuller; x4; Nuller]; [Nuller; x5; Nuller]; [Nuller; x6; Nuller];

% figur t = (0:delta_t:delta_t*(length(u1)1))'; tstop = N*delta_t + 2*sek_forst; u_sim = [t,u1,u2]; x_sim = [t,x1,x2,x3,x4,x5,x6];

% Virkelig tid

Page 29 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215

R2D = 180/pi; u1 = u1*R2D; u2 = u2*R2D; x1 = x1*R2D; x2 = x2*R2D; x3 = x3*R2D; x4 = x4*R2D; x5 = x5*R2D; x6 = x6*R2D;

%% feedback = 1; if feedback==1 %%%%%%%%%%%%%%%%% load heli_4_3_N80 %%%%%%%%%%%%%%%%% else %%%%%%%%%%%%%%%%% load heli_4_3_no_feedback_new %%%%%%%%%%%%%%%%% end

figure(1) subplot(8,1,1) stairs(t,u1) legend('Optimal pitch control $u_1^* = p_c^*$','location','SouthWest') ylabel('Degrees') fixplot; subplot(8,1,2) stairs(t,u2) legend('Optimal elevation control $u_2^* = e_c^*$','location','SouthWest') ylabel('Degrees') fixplot; subplot(8,1,3) plot( t,x1,'mo', rt_lambda(:,1),rt_lambda(:,2)); legend('Optimal travel $\lambda^*$','Real travel $\lambda$','location','SouthWest') ylabel('Degrees') ylim([200,200]); fixplot; subplot(8,1,4) plot(t,x2','mo', rt_r(:,1),rt_r(:,2)); legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthWest') ylabel('Degrees/sec') ylim([50,20]); fixplot; subplot(8,1,5) plot(t,x3,'mo', rt_p(:,1),rt_p(:,2)); legend('Optimal pitch $p^*$','Real pitch $p$','location','SouthWest') ylabel('Degrees') ylim([60,50]); fixplot; subplot(8,1,6) plot(t,x4','mo', rt_p_dot(:,1),rt_p_dot(:,2)); legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$','location','SouthWest') ylabel('Degrees/sec') ylim([200,200]); fixplot; subplot(8,1,7) plot(t,x5','mo', rt_e(:,1),rt_e(:,2)); legend('Optimal elevation $e^*$','Real elevation ylabel('Degrees') ylim([5,15]);

$e$','location','NorthWest')

Page 30 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243

fixplot; subplot(8,1,8) plot(rt_e_dot(:,1),rt_e_dot(:,2), t,x6','mo'); legend('Real elevation rate $\dot{e}$', 'Optimal elevation rate ... $\dot{e}^*$','location','SouthWest') xlabel('Time [s]') ylabel('Degrees/sec') ylim([20,20]); fixplot; set(gca,'XTickLabelMode','Auto'); set(gcf,'PaperType', 'A4') set(gcf,'PaperOrientation', 'portrait') papersize=get(gcf,'PaperSize'); set(gcf,'PaperPositionMode','manual') set(gcf,'PaperPosition',[0.25 0.25 papersize(1)0.5 papersize(2)0.5]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if feedback==1 createPdf('10_4_3_feedback_N80') else createPdf('10_4_3_no_feedback') end grid on set(gcf,'Position',[100,00 700,700])

2.4.1

Objective function

1 2 3 4 5 6 7 8 9

% objective.m function out = objective(z) global G out = z' * G * z; return

2.4.2

Nonlinear constraints

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18

% constraint.m function [c,ceq] = constraint(z) % c(z) <= 0 elevation constraint % ceq(z) = 0 not used global N mx mu beta alpha lambda_f c_tmp = Inf(N,1); lambda_k = z(1:mx:N*mx); ek = z(5:mx:N*mx); % Beregnet tilstand x1 % Beregnet tilstand x5 = elevasjon

for n = 1:N c_tmp(n) = alpha*exp(beta*(lambda_k(n)lambda_f)^2) ek(n); end ceq = []; c = c_tmp; end

Page 31 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

2.5
2.5.1

Accompanying scripts
Initialization le for helicopter 3

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50

% init.m %Denne fila inneholder initialisering for prosjektoppgave 2 i SIE3030. Den skal %brukes bare for helikopteret p forskshallen. clear all; clc; %%%%%%%%%%% Kalibrering av encoder og hwoppsett for aktuelt helikopter KalibVandring = .0879; KalibPitch = .0879; KalibElevasjon = .0879*1.5; EncoderInputVandring = 0; EncoderInputPitch = 1; EncoderInputElevasjon = 2; joystick_gain_x = 1; joystick_gain_y = 1; %%%%%%%%%%% Faste fysiske verdier gitt initielt m_w = 1.799; % motvekt m_h = 1.34; % helikopter l_a = 0.655; % lengde fra pivotpunkt til helikopterkropp l_h = 0.177; % lengde fra midten p helikopterkroppen til motor J_e = 2 * m_h * l_a *l_a; % treghetsmoment om elevasjonsaksen J_p = 2 * ( m_h/2 * l_h * l_h); % treghetsmoment om pitchaksen J_t = 2 * m_h * l_a *l_a; %treghetsmoment om vandringsaksen m_g=0.028; % Effektiv masse (med motvekt) K_p = m_g*9.81; % Kraften som trengs for holde helikopteret i likevekt. V_f_eq = 1.2; %Bytt verdi slik at det stemmer med det aktuelle helikopteret V_b_eq = .85;%1.2; %Bytt verdi slik at det stemmer med det aktuelle helikopteret V_s_eq = V_f_eq+V_b_eq; % Minimum spenning for holde helikopteret i likevekt K_f = K_p/V_s_eq; % Kraftkonstant (N/V) %K_ep = 15.1243; %K_ed = 9.3779; %K_ei = 1.4216; % K_ep % K_ed % K_ei K_ep = K_ed = K_ei = K_1 K_2 K_3 K_4 = = = = = 15; = 11.5; = 2.3; 6.3;5.5; 10; 2.1; l_h*K_f/J_p; K_p*l_a/J_t; K_f*l_a/J_e; K_p*l_a/J_e;

w_c = 5; K_pd = w_c/K_1; K_pp = (sqrt(2)*w_c^2)/K_1;

Page 32 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

2.5.2

A Enable L TEX interpretation of plot legends

1 2 3 4 5 6 7 8 9 10 11

% fixplot.m grid on h=legend; set(h, 'interpreter', 'latex'); set(gca,'xticklabel',[]); p = get(gca,'position'); p(4) = p(4)*1.25; set(gca, 'position', p);

2.5.3

Automate creation of PDF plots

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21

% createPdf.m function [] = createPdf(filename, enable) try grid on h=legend; set(h, 'interpreter', 'latex'); if nargin == 1 enable = 1;

end if enable > 0 if isdir('pdf') == 0 mkdir('pdf'); end print('painters','dpdf','r900', ['pdf//', filename] ); end catch err warning(err.message); end

Page 33 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

C
3.1

Simulink Diagrams
Simulink diagram for Task 2.4

lambda Bad Link r u_sim From Workspace u

p_c, rad

-CHIL Initialize
??? (???-???)

lambda_0
Vandring, (grader) Vandringsrate, (grader/sek) V_f, motor foran Pitch, (grader)

p_dot

Terminator Terminator1

p, rad

Out2

V_d

V_f

p_dot, rad/sek Pitchrate, (grader/sek)

K*u Gain

Pitch-kontroller
V_s e, rad e_dot, rad/sek V_s Elevasjonsrate (grader/sek) V_b V_b, motor bak Elevasjon, (grader)

V_d/V_s --> V_f/V_b -Celevation

e_c, rad

Heli 3D -30

Elevasjonskontroller Constant

Add

Figure C.1: Simulink diagram for Task 2.4.

3.2

Simulink diagram for Task 3.2

Bad Link K K u_sim x_sim

p_c, rad

Matrix Multiply Mult HIL Initialize

??? (???-???)

lambda

r -Clambda_0
Vandring, (grader)

p_dot
Out2 V_d V_f Vandringsrate, (grader/sek) V_f, motor foran Pitch, (grader)

p, rad

p_dot, rad/sek Pitchrate, (grader/sek)

K*u Gain

Pitch-kontroller
V_s e, rad e_dot, rad/sek V_s Elevasjonsrate (grader/sek) V_b V_b, motor bak Elevasjon, (grader)

V_d/V_s --> V_f/V_b -Celevation

e_c, rad

Heli 3D -30 Constant e

Elevasjonskontroller

Figure C.2: Simulink diagram for Task 3.2.

Page 34 of 35

NTNU Department of Engineering Cybernetics

Helicopter lab

3.3

Simulink diagram for Task 4.3

Bad Link

lambda

K K u u_sim
p_c, rad

Matrix Multiply Mult

HIL Initialize
??? (???-???)

r -Clambda_0
Vandring, (grader)

p_dot

x_sim
p, rad Out2 V_d V_f Vandringsrate, (grader/sek) V_f, motor foran Pitch, (grader) p_dot, rad/sek Pitchrate, (grader/sek)

K*u Gain

Pitch-kontroller
V_s e, rad e_dot, rad/sek e_c, rad V_s Elevasjonsrate (grader/sek) V_b V_b, motor bak Elevasjon, (grader)

V_d/V_s --> V_f/V_b

Heli 3D -30 Constant e e_dot

Elevasjonskontroller

Figure C.3: Simulink diagram for Task 4.3.

Page 35 of 35