Department of Physics Ume˚ a Universitet

January 19, 2015

A motion cueing model for mining and forestry simulator platforms based on Model Predictive Control

En modell f¨ or r¨ orelsesignaler till simulatorplattformar inom gruv- och skogsindustri baserad p˚ a Model Predictive Control

Oryx Simulations AB

Jens Walker [email protected]

Master thesis 30hp Examiner: Eddie Wadbro ([email protected]) Supervisor: David Grundberg ([email protected])

Jens Walker [email protected]

January 19, 2015

Abstract Oryx Simulations produce simulators for mining and forestry machinery used for educational and promotional purposes. The simulators use motion platforms to reflect how the vehicle moves within the simulator. This platform tilts and accelerates the driver in order to enhance the experience. Previously a classical washout filter algorithm has been used to control the platform which leaves something to be desired regarding how well it reflects the vehicles movement, how easy it is to tune and how it handles the limits of the platform. This thesis aims to produce a model that accurately reflects angles, velocities and accelerations while in the mean time respecting the limits of the platform. In addition to this the developed model should be easy to modify and tune. This is achieved using so-called Model Predictive Control which achieves the wanted behaviour by predicting how the platform will move based on its current state while implementing the constraints of the platform directly into the model. Since all of the parameters in the model are actual physical quantities, this makes the model easier to tune. A key component in this solution is the so-called tilt coordination which consists of substituting a lateral/longitudinal acceleration with the acceleration of gravity by tilting the driver. Constructing and implementing this model in Matlab we verify it by using data extracted from the simulator environment. We see that the parameters consisting of angles, rotational velocities and linear accelerations are tracked very well while respecting the constraints for the platform, constraints that can be easily changed to fit the current simulator. We also see that the model successfully implements tilt coordination into the behaviour of the platform. This model performs extraordinarily well in theory, what remains is to implement this to the motion platform and fine-tune it.

i

Jens Walker [email protected]

January 19, 2015

Sammanfattning Oryx Simulations tillverkar simulatorer i huvudsak f¨ or gruv- och skogsindustrin vilket anv¨ ands i utbildnings- och marknadsf¨ oringssyfte. Simulatorerna anv¨ ander en r¨ orelseplattform f¨ or att spegla hur fordonet i simulatormilj¨ on r¨ or sig. Denna plattform lutar och accelererar f¨ oraren f¨ or att f¨ orst¨ arka upplevelsen. Tidigare har ett s˚ a kallat klassiskt washoutfilter anv¨ ants f¨ or att kontroller plattformen som l¨ amnar en del i o ¨vrigt att o ¨nska vad g¨ aller hur v¨ al fordonets r¨ orelser speglas, hur l¨ att det ¨ ar att justera samt hur det hanterar plattformens begr¨ ansningar. Detta projekt a ¨mnar producera en modell som v¨ al speglar vinklar, hastigheter och accelerationer samtidigt som den respekterar plattformens gr¨ anser. I till¨ agg till detta b¨ or modellen vara enkel att modifiera och justera. Detta uppn˚ as genom s˚ a kallad Model Predictive Control som f¨ oruts¨ ager hur plattformen kommer r¨ ora sig utifr˚ an dess aktuella tillst˚ and samtidigt som den respekterar de tv˚ ang som finns p˚ a plattformen direkt i modellen. D˚ a alla parametrar i modellen a ¨r faktiska fysiska kvantiteter blir modellen m¨ arkbart l¨ attare att justera. En viktig komponent i denna l¨ osning a ¨r s˚ a kallad tilt coordination vilket best˚ ar i att substituera lateral/longtudinell acceleration med en komposant av tyngdaccelerationen genom att luta f¨ oraren. Denna modell konstrueras och implementeras i Matlab och verifieras genom att anv¨ anda extraherat data fr˚ an den simulerade milj¨ on. Vi kan se att parametrarna som best˚ ar av vinklar, rotationella hastigheter och linj¨ ara accelerationer speglas v¨ aldigt v¨ al, samtidigt som tv˚ angen p˚ a plattformen respekteras. Dessa tv˚ ang kan enkelt modifieras f¨ or att passa den aktuella simulatorn. Vi ser a ¨ven att modellen framg˚ angsrikt implementerar tilt coordination i plattformens beteende. I teorin har denna modell v¨ aldigt bra prestanda; vad som kvarst˚ ar ¨ ar att implementera den p˚ a en r¨ orelseplattform och finjustera modellen.

ii

Jens Walker [email protected]

January 19, 2015

Acknowledgements Firstly I would, clich´e enough, like to thank my family. Their support and encouragement has never wavered even though I have neglected them for long periods of time during my years at the university. Equally I want to sincerely, from the bottom of my heart, thank Emma and Maria for being never-ending sources of laughter, knowledge, support and wisdom not only during long and late hours studying, but in my life in general. Further I would like to express my gratitude to Krister for being so dedicated to the students and going to great lengths to enable me and others to pursue interests strictly not within the bounds of the programme. Last but definitely not least, I want to thank Tony for always being a sober influence, even when not particularly sober himself, without whom I would not have studied engineering physics to begin with.

iii

Jens Walker [email protected]

January 19, 2015

Nomenclature Notations ()oto

Variable related to the otolith model

()scc

Variable related to the semicircular canals model

()rot

Variable related to the rotational model

()lin   .. .

Variable related to the linear acceleration model Matrix with m rows and n columns

m×n

Parameters l1 , l2 xi , yi αi a β βi θ φ ψ ω ωi θ˙ φ˙ ψ˙ a ai ax ay az g F Np Nc yref Nx Nu Ny J

Actuator lengths Actuator end positions, i=1,2 Actuator angles, i=1,2 Fix position in x for actuator 2 Vector containing the Euler angles i:th element of β Roll Euler angle, β1 Pitch Euler angle, β2 Yaw Euler angle, β3 Vector containing the angular velocities i:th element of ω Roll velocity, ω1 Pitch velocity, ω2 Yaw velocity, ω3 Vector containing the accelerations i:th element of a Acceleration in x-direction, a1 Acceleration in y-direction, a2 Acceleration in z-direction, a3 Acceleration of gravity Forces of the system Prediction horizon Control horizon Output reference vector Number of state parameters Number of control parameters Number of output parameters Cost function

iv

Jens Walker [email protected]

January 19, 2015

Matrices 0m×n Im×m A B C x y u ξ Y U ∆U Ymax/min Umax/min ∆Umax/min b M E Qm Sm Rm Q S R H f Φ κ

m-by-n matrix of zeros m-by-m identity matrix State matrix of the state space model Input matrix of the state space model Output matrix of the state space model State vector Output vector Control vector Augmented state vector Augmented output constraint vector Augmented control sequence constraint vector Augmented control sequence rate constraint vector Maximum/minimum output constraint vector Maximum/minimum control sequence constraint vector Maximum/minimum control sequence rate constraint vector Composite constraint vector Composite constraint matrix System error matrix Output weight matrix Control sequence weight matrix Control sequence rate weight matrix Composite output weight matrix Composite control sequence weight matrix Composite control sequence rate weight matrix Hessian matrix Gradient vector MPC gain matrix Optimal formulation matrix

v

Jens Walker [email protected]

January 19, 2015

Contents 1 Introduction 1.1 Background 1.2 Purpose . . 1.3 Goal . . . . 1.4 Limitations

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

1 1 1 2 2

2 Theory 2.1 Motion platforms . . . . . . . . . . . . . . 2.1.1 Stewart platform - 6DOF . . . . . 2.1.2 Car simulator platform - 2DOF . . 2.1.3 Rotational platform - 2DOF . . . . 2.2 The Oryx platform . . . . . . . . . . . . . 2.3 Vestibular system . . . . . . . . . . . . . . 2.3.1 Semi-circular canals . . . . . . . . 2.3.2 Otoliths . . . . . . . . . . . . . . . 2.4 Washout algorithm . . . . . . . . . . . . . 2.4.1 Classical washout algorithm . . . . 2.4.2 Adaptive washout algorithm . . . 2.4.3 Optimal washout algorithm . . . . 2.5 Introduction to Model Predictive Control 2.5.1 MPC - an example . . . . . . . . . 2.5.2 MPC mathematical concept . . . . 2.6 Obtaining MPC matrices . . . . . . . . . 2.6.1 Tilt coordination . . . . . . . . . . 2.6.2 State, output and control vector . 2.6.3 System matrices . . . . . . . . . . 2.7 Optimal form MPC reformulation . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . .

3 3 3 3 3 3 7 8 8 9 9 9 9 10 10 11 12 12 12 14 17

3 Method 3.1 Analysis of current implementation 3.2 Constructing code for evaluation . 3.3 Calibrating the MPC model . . . . 3.4 Extracting data from the simulator

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

22 22 22 24 25

4 Experiments and results 4.1 Validating platform model . . . . . . . . . . . 4.2 Validating the MPC model . . . . . . . . . . 4.2.1 Tilt coordination evaluation . . . . . . 4.2.2 Constraint handling evaluation . . . . 4.2.3 Accuracy evaluation . . . . . . . . . . 4.3 Sources of error . . . . . . . . . . . . . . . . . 4.4 Possible expansions . . . . . . . . . . . . . . . 4.4.1 Implementation and online calibration 4.4.2 Convert to a sparse system . . . . . . 4.4.3 Vehicle model . . . . . . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

27 27 29 29 30 32 34 34 34 34 34

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

vi

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

Jens Walker [email protected]

January 19, 2015

5 Conclusions 35 5.1 Precision of the model . . . . . . . . . . . . . . . . . . . . . . . . 35 5.2 Tilt coordination . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 5.3 Numerical feasibility . . . . . . . . . . . . . . . . . . . . . . . . . 35 A Appendix - Coefficients used

36

B Appendix - Platform measurements

38

C Appendix - Complete C.1 Circular path . . . C.2 Extreme tilt . . . . C.3 Scoop slam . . . .

model output 39 . . . . . . . . . . . . . . . . . . . . . . . . . . 39 . . . . . . . . . . . . . . . . . . . . . . . . . . 42 . . . . . . . . . . . . . . . . . . . . . . . . . . 45

vii

Jens Walker [email protected]

1 1.1

January 19, 2015

Introduction Background

Oryx manufactures sophisticated simulation equipment (henceforth simulators) primarily for the mining and forestry industry, which are used for training and marketing purposes. The equipment normally consists of a computer system and a number of displays mounted on a motion platform (after this referred to as mpf). This platform reproduces the tilt of the vehicle and inertial forces that occur during acceleration and rapid turning to enhance the user experience. A completely holonomic system would have 6 degrees of freedom (DOFs). These are the linear movements in x- y- and z-direction called sway, surge and heave respectively as well as rotation about these axis called roll, pitch and yaw. In this application there are 5 DOFs of interest. These are the 3 linear translations and the two rotations pitch and roll. Yaw is considered to be of less importance for most of the vehicles that Oryx simulate since the driver is rarely positioned directly at the yaw axis. This is important since the movement is then perceived as an acceleration sideways rather than a rotation. This report is the result of a master thesis project in engineering physics performed by Jens Walker at Ume˚ a University in collaboration with Oryx Simulations AB. The project is supervised and graded by Eddie Wadbro ([email protected]) at the department for Computing Science at Ume˚ a University. Supervisor at Oryx is David Grundberg ([email protected]).

1.2

Purpose

The model that is currently implemented to control the platform uses very basic techniques, so a new, more complex model is desired in order to be able to better track the movement of the simulated vehicle. Currently the model implemented does not reflect linear accelerations in a satisfactory manner partly because the current design of the motion platform does not allow for planar motion and hence the inertial forces cannot be recreated directly. One of the concepts used to solve this is called tilt coordination which substitutes linear acceleration for acceleration of gravity. Another drawback with the current method is that configuration is time consuming and possibly causes different simulators to behave differently depending on when it was configured and who performed the configuration. It can also result in different behaviours for different velocities of the simulated vehicle, since what is considered ”normal operating velocity” is likely to be tested more thoroughly. Thus, in order to save time and get a more homogeneous configuration for the simulators, an easily-tuned mathematical model is to be developed. A further benefit of a well-defined and documented model for the movement of the mpf is that it would simplify expansion. This could include the addition of further effects such as idling movement, i.e the vibrations caused by a running engine or the sensation of different terrains.

1

Jens Walker [email protected]

1.3

January 19, 2015

Goal

The goal of this project is to develop a mathematical model that accurately reflects the angles, velocities and accelerations of the vehicle while simultaneously taking into account the limited range of the mpf. Another key aspect of the model is that tilt coordination should be present. The model should be optimized for numerical solving and a prototype code should be constructed.

1.4

Limitations

The model will be developed and investigated in theory only and not implemented on the platform. A basic version of the model will be investigated. This means that the model will be adapted to the system with no extra functionalities added. Also any relevant software packages whose license permits it to be used in a commercial setting will be used.

2

Jens Walker [email protected]

2

January 19, 2015

Theory

By using concepts of physics and mathematics, we will in this section derive everything needed in order to find a functional model for determining how the mpf will move. To do this we need a brief background of different motion platforms, how humans detect movement and also what algorithms has been used before. When we have a handle on these topics, we delve deeper into the underlying theory of the model that we are to tailor to our system.

2.1

Motion platforms

A motion platform allows the user to be moved, tilted and rotated in different ways to raise the perceived level of realism in a simulator. There is a multitude of different platforms, but there are three configurations that are the most common. 2.1.1

Stewart platform - 6DOF

A platform with 6 DOF’s, achieved by using six independent actuators. The drawbacks are that it is complicated to control and have a very limited range. This platform is often used for more high-level applications where extreme forces occur and accuracy is of the essence, e.g. fighter plane simulators. 2.1.2

Car simulator platform - 2DOF

As the name suggests, this is often used in car simulators and have two degrees of freedom. The seat can rotate about its z-axis (yaw) and is positioned on a rail so that it can slide forward/backward so as to emulate acceleration of a vehicle. If the yaw angle is non-zero the driver will also be accelerated to the side, which is used to emulate the feeling of changing lanes. 2.1.3

Rotational platform - 2DOF

This type of simulator is often used where the tilt of the platform is of great importance but the lateral forces of the simulated vehicle are small, e.g. ships, enthusiast aircraft simulators and, as in our case, mining vehicle simulators.

2.2

The Oryx platform

We now turn our focus to our specific platform, which is a rotational platform as described in section 2.1.3. The platforms at Oryx utilize a universal joint in the rear center of the platform and two electrical stepper motors at the front, each lifting one of the front corners, see figure 1. This gives us the pitch and roll directly.

3

Jens Walker [email protected]

January 19, 2015

Figure 1: CAD sketch of the platform

The lifting mechanism of the platform consist of a lever arm secured to the motor axis which in turn is connected to the ball joint of an actuator. The actuator is in turn connected to the platform through another ball joint. This setup can be seen in figure 2.

Figure 2: CAD sketch of the motor/actuator setup

4

Jens Walker [email protected]

January 19, 2015

An hypothesis is that this system can be modelled as a double pendulum with a boundary condition. In figure 3 we can see how this model would look schematically. The length l1 is the length of the lever arm from the motor axis to the center of the lower ball joint, while l2 is the length of the actuator from center to center of the ball joints.

Figure 3: Schematic view of the double pendulum model

This model gives us the system of equations: x1 = l1 sin(α1 )

(1)

y1 = −l1 cos(α1 )

(2)

x2 = a

(3)

y2 = −l1 cos(α1 ) − l2 cos(α2 ))

(4)

In (3) we have applied the fact that the outer end of the actuator cannot move in the x-direction since it is secured to the platform. Hence the coordinate x2 is fix at the position a which simplifies the system and allows us to numerically compute the angle α2 . This gives us the coordinate y2 which is the displacement of the platform in terms of the motor angle α1 , which is a parameter we control. Constructing a wire frame model of the platform using measurements obtained from the CAD sketch, we get figure 4. This wireframe model is later used to calculate the values of our simplified model.

5

Jens Walker [email protected]

0.2

January 19, 2015

0 −0.2

−0.4

−0.6 −0.8

0.8

0.7

0.6

0.5

0.4

0.3

0.2

Y Figure 4: Wire frame model of the motion platform X

Assuming that we know the geometry of the platform as well as the angles for the roll, θ, and and pitch, φ, we can find a relation between the platform angles and the motor angle. Using the x-distance from the center of the platform to the front upper joint, denoted xp and the distance in from the rear joint to the front joints, denoted yp . We can then find the motor angle for the roll using α1roll =

y2 1 tan−1 ( ) , 2 xp

(5)

1 where the comes from the fact that the angle for the motor on the other side 2 of the platform moves in the other direction, i.e. has the same equation but with a negative sign. In the same way we find the motor angle for the pitch, but instead we use the distance to the rear joint, α1pitch = tan−1 (

6

y2 ). yp

(6)

Jens Walker [email protected]

2.3

January 19, 2015

Vestibular system

The human vestibular system is what allows us to perceive accelerations and therefore movements. It is positioned in the inner ear and comprised of the semicircular canals and otoliths. A schematic image is depicted in figure 5. Slightly simplified, the semicircular canals are pipes partially filled with fluid that, as we rotate, put pressure on a membrane which in turn send signals to the brain telling us that we are indeed rotating. The otoliths behave in a similar fashion, but rather than canals it consists of gelatinous blobs called the utricle and saccule encapsulating hair cells that trigger when experiencing linear accelerations.

Figure 5: Schematic image of the human vestibular system [1]

All inertial forces originally stem from an acceleration and these can be either from changing the velocity or a rapid turn at high velocity. In the latter case it comes from the acceleration needed in order to change direction. An inertial force is always experienced in the direction opposed to the acceleration. Since the origin of inertial forces are planar accelerations which the mpf is not capable of, we need to trick the user that these displacements do actually happen. For low-frequency accelerations this is normally achieved by tilting the user and exploiting the components of the gravitational acceleration parallel to the platform and is called tilt coordination. If the users vision is immersed in a virtual environment, the human vestibular system is not sophisticated enough to know the difference. After the inertial force has been emulated in this manner the platform position need to be restored since otherwise the tilt would be incorrect, but also to be able to repeat another acceleration without violating the limitations of the platform.

7

Jens Walker [email protected]

January 19, 2015

This needs to be translated into a mathematical model and thankfully this subject is well investigated and so-called transfer functions in state-space for both the otoliths and semi-circular canals are available [3]. 2.3.1

Semi-circular canals

Defining the vector β as the angles of the platform, with the following components  T β= θ φ ψ , (7) where θ is rotation around the x-axis (roll), φ is rotation around y (pitch) and ψ is rotation around z (yaw). Further we define the time derivative of β  T ω = β˙ = θ˙ φ˙ ψ˙ (8) which is the rotational velocity retrieved from the simulator, where x˙ signifies the derivative of the general variable x w.r.t. time. As done in [8] we can then use a transfer function in order to obtain the perceived rotational velocity ω ˆ where each element is found as ωˆi =

(Tai s

Tli Tai s2 ωi . + 1)(Tsi s + 1)(Tli s + 1)

(9)

˙ we substitute T i , T i , T i for the values That is if we look at the roll velocity θ, a s l x x x of Tl , Ts , Ta found in Appendix A and so on. Equation (9) can be rewritten in its general form as ωˆi =

s3 + ( T1i + a

1 Tli

+

1 2 Tsi )s

+

1 2 Tsi s ( T i1T i + T i1T i l s l a

+

1 Tsi Tai )s

+ ( T i T1i T i ) a

l

ωi ,

(10)

s

which gives the whole transfer function as s multiplied by some coefficients, which is needed for the canonical state representation introduced later. 2.3.2

Otoliths

Similarily, there exists a transfer function for the linear accelerations, which is perceived by the otoliths. If we define   fx f = fy  , (11) fz then the transfer function looks as follows: fˆi =

K i (τai s + 1) fi . (τli s + 1)(τsi s + 1)

(12)

As we did in (10), we can rewrite the transfer function (12) to general form according to K i τai K i τai s+ τl τs τl τs f . (13) fˆi = 1 1 1 i 2 s + ( i + i )s + i i τs τl τl τs

8

Jens Walker [email protected]

2.4

January 19, 2015

Washout algorithm

The traditional approach to the problem of how to move a platform in sync with a simulator is to use a washout-algorithm, which reflects in [9] where the only method listed for implementation is in fact the washout filter approach. This is a series of low-pass and high-pass filters that act to pick out the components of acceleration and translate this to how the platform should move. Within these filters are coefficients that regulate the cut-off and pass frequencies which determine the behaviour of the algorithm. There are several different versions of this algorithm, what they have in common is that they all involve a fair amount of signal processing and filtering techniques. The algorithm currently used at Oryx is the classical washout algorithm. For more information on this approach, feel free to read [10] by C.D. Larsen. 2.4.1

Classical washout algorithm

Classical washout uses fix coefficients which need to be tuned offline, that is with the simulator turned off, and are always kept fixed. This is an issue since you need to configure these according to a worst-case scenario thus not utilizing the whole range of the platform. It is also a difficult process to get these right since they do not represent any physical quantity directly but rather represent frequencies. 2.4.2

Adaptive washout algorithm

Adaptive washout filters uses an approach where the coefficients vary with time depending on position and velocity of the platform. This improves upon the classical algorithm not only in performance, but also simplifies the tuning slightly, since this can be done online with the simulator running. 2.4.3

Optimal washout algorithm

Optimal washout uses the aforementioned transfer functions of the human inner ear in order to translate the acceleration and rotation from the simulator to actual sensed movement using a cost function to minimize the error between the two. Again we face the problem of having to tune to the best overall performance with the trade-off that we do not use the platform optimally. Further, the problem appears again with the coefficients that has no physical representation, thus still making the tuning process complicated.

9

Jens Walker [email protected]

2.5

January 19, 2015

Introduction to Model Predictive Control

While there is still performance gains in implementing one of the more advanced washout algorithms, a lot of progress has recently been made using so called Model Predictive Control, or MPC. This approach has been used within the chemical industry since the 80’s but has recently been adapted to other areas, such as motion cueing [11]. This is a process for predicting how a system will move ahead of time for a certain duration, the so-called prediction horizon. This enables maximized exploitation of the platforms range since at each time step we know how much space we can use without hitting the boundaries, since it also incorporates the constraints directly in the system. It takes a similar approach to the optimal washout algorithm, in that it uses a transfer function to model how the accelerations and rotations should be enacted on the platform as well as a cost function to minimize error. Further it uses linear algebra to solve a matrix system in order to predict how the simulated vehicle will move in the nearby future as opposed to signal analysis and filtering. This makes it more appropriate for this project since the degree for which this thesis project is performed is focused more on matrix computation than signal analysis. 2.5.1

MPC - an example

In order to understand the MPC model, we look at a real life example: the production at an assembly line. Every morning at 8.00 we have a meeting deciding how many units we produce, say 100 each hour, for the next 4 hours until 12 o’clock. Thus our reference function is 100 units per hour and our prediction horizon Np is 4 hours. The state x is information about the line at 8 o’clock and can for example be how large a workforce is available right now and how fast the assembly line is moving, while we constantly have to take into consideration different constraints such as how much materials we have. The control sequence u is what we change until next meeting and can be if we decide to hire more people, buy more materials and if we speed up or down the assembly line. Furthermore the output y in this example is how many units we produce which is limited by for example how much materials we have and maximum speed of conveyor belts which is our constraints. When we meet again at 9 o’clock, we take a look at how many units we actually produced during the last hour, let us say that we produced 104 which is then our actual output yˆ. Here we overshot a bit, so we then agree to lower the speed of the assembly line, that is change our control sequence u slightly and make a new prediction from 9.00 to 13.00, thus discarding the rest of our previous prediction. Now let us also assume that our state x indicates that we are starting to run low on a certain type of material and only have 90 left, that is we are being limited by one of our constraints. Since this is included directly in the model, then in our next calculation for production rate this will be taken into consideration automatically. So, production rate will be lowered for the next hour to say 85 units, so as not to break this condition which in our case means run out of bolts

10

Jens Walker [email protected]

January 19, 2015

before the hour is up leaving people without tasks to perform. However, if we get a delivery by our next meeting at 10 o’clock, the MPC model compensates for the lowered production and will increase production rate for the nearest prediction to say y = 95, 106, 111 and 103 units for each of the coming four hours respectively. This is the strength of the model, since it adapts to the constraints on the fly and allows us to be very close to the limits of our constraints without breaking them and ensures a smooth behaviour even when close to the constraints. 2.5.2

MPC mathematical concept

More formalized, we use the state x of the system to predict what our future control sequence u should be in order to minimize the error between the predicted output y and a reference function yref . The reference function yref can in some sense be seen as the ”correct” result and normally these parameters are set to be equal to the desired parameters, i.e to the parameters obtained from the simulator. However, this can be exploited so that if we want a certain parameter to always strive towards zero, we set that parameter in the reference function to 0. This can be seen as a built-in washout filter and is especially useful for the angles since we want them to return to their neutral position to be able to perform more drastic manoeuvres. We also denote Nx , Ny and Nu as the number of state, output and control sequence parameters respectively. We then take the first control sequence u and send to our system while discarding the rest, since new ones will be calculated in the next time step. This whole problem can be described as a differential equation x˙ = Ax+Bu , y = Cx + Du ,

(14)

where for multiple input, multiple output (MIMO) systems, A, B, C and D are matrices containing coefficients for our specific model, in our case derived from the transfer functions. Here we assume a strictly proper system, which means that the input we send now can not affect the current state. So in a discrete space, let k denote the current time step and u(k) and y(k) denote u and y at a time tk . Then u(k) cannot affect y(k) but only the future states y(k+1), y(k+2),..., y(k+Np ). This means that D=0, which gives us (15) x(k+1) − x(k) = Ax(k) + Bu(k) y(k) = Cx(k).

(15)

Since we would like to impose constraints for both y, u and the rate with which u changes, denoted ∆u, we need to find a way to optimize for all of the variables simultaneously. The constraints on the output y is enforced during our prediction horizon Np , while for the control sequence they are applied during what is called the control horizon, denoted Nc . Note that Nc ≤ Np , since we cannot control our output ahead of where we know it.

11

Jens Walker [email protected]

2.6

January 19, 2015

Obtaining MPC matrices

The next step is to specify the x, y and u vectors as well as the A, B and C matrices according to our model. Our system is actually a composite of four parts, one for the semicircular canals with the subscript scc, one for the otoliths subscripted oto, one for the angles subscripted rot and one for the linear movements subscripted lin. We start off by defining tilt coordination, then the composite system and will later specify what the matrices contain. Deciphering this system is quite the rabbit hole that we dive down into, so please bear with all of the declarations and definitions of new matrices.

2.6.1

Tilt coordination

In order to infuse our model with the desired tilt coordination, we need to establish a model for this as well. Using Newtons second law that F = ma ,

(16)

we can add the contribution of gravity to our model. Since the force of gravity is strictly linear, it becomes a contribution to the otolith model. If we add the contribution of gravitational forces to the ordinary linear forces, we get   ax − g sin(φ) (17) F = m ay − g cos(φ) sin(θ) az − g cos(φ) cos(θ) We then assume that both θ and φ are small which gives us the small angleapproximation:   ax + gφ (18) F˜ = m  ay − gθ  . az − g We can now separate this into a gravity matrix G and a coordinate vector as   θ  φ      0 g 0 1 0 0  ψ β   ˜   F = m −g 0 0 0 1 0   = m G . (19) a a x 0 0 0 0 0 1  | {z } ay  G az 2.6.2

State, output and control vector

Even though these vectors are established along the way when assembling the matrices, for the sake of understanding we will state them beforehand. This will hopefully make it easier to understand why some of the matrices look like they do. The state, output and control vectors are composed as   xscc x = xoto  , (20) xlin N ×1 x

12

Jens Walker [email protected]

January 19, 2015

  y scc y oto   , y= y rot  y lin N ×1 y   u u = scc uoto N ×1

(21)

(22)

u

respectively. Here we can clearly see which parameter that belongs to a certain aspect of the model. Stating each element, we get    ˙ ax θ  θ˙  0       px ay  0  vx         φ˙   φ˙  py           (23) xlin =  xoto = az  , xscc =  0  ,  vy  .    ψ˙  0      pz  θ ψ˙      vz φ 0 ψ 0 The elements of the output vector y that contains the processed values looks as follows   px  vx   ˙       θ ax θ py   y scc =  φ˙  , y oto = ay  , y rot =  φ  , y lin =  (24)  vy  .   ˙ a ψ ψ z pz  vz As for the control sequence vector u, we have  ˙   θ ax uscc =  φ˙  , uoto = ay  . az ψ˙

(25)

This means that for our model we can specify Nx =24, Ny =15 and Nu =6. We can now also explicitly state the vectors y, u and ∆u as: θ˙

y=



u=



θ˙

∆u =



∆θ˙

φ˙ φ˙

ψ˙ ψ˙ ∆φ˙

ax ax ∆ψ˙

ay ay

az

θ

az

T

∆ax

φ

ψ

px

vx

py

vy

pz

vz

T

,

,

∆ay

∆az

T

. (26)

13

Jens Walker [email protected]

2.6.3

January 19, 2015

System matrices

We then move on to the matrices. It may not be apparent at first sight why they look like they do, but it will hopefully become clear further down. We define them as 

 09×9 09×6 Aoto 09×6  , 06×9 Alin N ×N x x   B scc B = B oto  B lin N ×N

Ascc A = 09×9 06×9

x

and



C scc C = 03×9 09×9

03×6 C oto 09×6

(27)

(28)

u



03×9 03×9  C lin N

.

(29)

y ×Nx

Now, let us decipher these matrices! We start with C, then work our way through B and finish with A. The reason for this is that we need to do perform a few tricks to implement the tilt coordination. So, let’s start with C   x C scc 01×3 01×3 , (30) C scc = 01×3 C yscc 01×3  01×3 01×3 C zscc 3×9 where the block matrices of the scc-model are in themselves composed of matrices and look like  C iscc = 1 where i is substituted for the coordinate on to the otolith model, we have  x C oto 01×2 C oto = 01×2 C yoto 01×2 01×2

0

 0 ,

(31)

of interest, that is i = x, y, z. Moving 01×2 01×2 C zoto

 01×3 01×3  , 01×3 3×9

where each block matrix in the otolith matrix looks as   C ioto = 1 0 .

(32)

(33)

while the matrix Clin is simply a 9×9 identity matrix C oto = I9×9 .

(34)

Now we look at the matrix B and start with the scc-part which is fairly straightforward.

14

Jens Walker [email protected]

B scc

January 19, 2015

 x B scc = 03×1 03×1

03×1 B yscc 03×1

 03×(Nu −3) 03×(Nu −3)  03×(Nu −3) 9×N

03×1 03×1 B zscc

,

(35)

u

where the right hand part of zeros is inserted since in the scc-model we want to use the angular velocities and not the accelerations contained in the control sequence vector u. Each block matrix in the model is defined as  1 −Ti s i  (36) B scc = 0  , 0 where again i = x, y, z. Moving on to the otolith model, we have to use some fancy footwork in order to implement the tilt coordination. We start by defining " Kτ i # a

τli τsi K τli τsi

B ioto =

,

(37)

which we get from the observable canonical state space form of the transfer function as according to [5]. Since the angles are included in the state vector x and the linear accelerations in the control vector u, we need to pair the matrix G obtained in (19) with the otolith model, which is done using the matrix BG  x   B oto 02×1 02×1 0 g 0 1 0 0 B G = 02×1 B yoto 02×1  −g 0 0 0 1 0 , (38) 02×1 02×1 B zoto 0 0 0 0 0 1 which, after performing the  02×1 B G = −gB yoto 02×1

multiplication becomes gB xoto 02×1 02×1

02×1 02×1 02×1

B xoto 02×1 02×1

02×1 B yoto 02×1

 02×1 02×1  . B zoto 6×6

(39)

However, we need to split the matrix in (39) in two, one part that belongs to the angles and one that belongs to the accelerations.   θ φ     ψ β  BG = BG  (40) ax  = B β β + B a a , a    ay  az We then extract the matrices Bβ and  02×1 Bβ = −gB yoto 02×1  x B oto Ba = 02×1 02×1

Ba to obtain gB xoto 02×1 02×1 02×1 B yoto 02×1 15

 02×1 02×1  , 02×1 6×3  02×1 02×1  . B zoto 6×3

(41)

(42)

Jens Walker [email protected]

We then use Ba for B, obtaining  02×3 02×3 B oto =  02×3 I3×3

January 19, 2015

B xoto 02×1 02×1 03×1

while we need to associate Bβ with state vector x. First we define  0 0 0 0  0 0 B lin =  0 0  0 0 0 0

02×1 B yoto 02×1 03×1

 02×1 02×1   B zoto  03×1 9×N

,

(43)

u

A, since the angles are contained in the 0 0 0 0 0 0

0 1 0 0 0 0

0 0 0 1 0 0

 0 0  0  0  0 1 6×N

,

(44)

u

which is just to pass along the linear part of the control sequence vector. To construct A we start by defining  x  Ascc 03×3 03×3 Ascc = 03×3 Ayscc 03×3  , (45) 03×3 03×3 Azscc 9×9 where we again find the submatrices by converting the transfer function (10) into observable canonical state space form according to [5], which gives us   −( T1i + T1i + T1i ) 1 0 a s l   1 1 1 Aiscc = −( Tli Tsi + Tli Tai + Tsi Tai ) 0 1 , (46) −( T i T1i T i ) 0 0 a

s

l

where i = x, y, z allows us to obtain the submatrices. We now augment the matrix Aoto with Bβ , which gives us   x Aoto 02×2 02×2 02×1 gB xoto 02×1 02×2 Ayoto 02×2 −gB yoto 02×1 02×1   . (47) Aoto =  02×2 02×2 Azoto 02×1 02×1 02×1  03×2 03×2 03×2 03×1 03×1 03×1 9×9 As before we find the submatrices by converting the transfer function in (13) to canonical state space " 1 # 1 − − 1 i i τ τl s Aioto = , (48) − τ i1τ i 0 l

s

where again i = x, y, z. Finally, as for B, the linear submatrix for A just passes along the linear part of the state vector by being defined as   0 1 0 0 0 0 0 0 0 0 0 0   0 0 0 1 0 0  Alin =  . (49) 0 0 0 0 0 0   0 0 0 0 0 1 0 0 0 0 0 0 6×6

16

Jens Walker [email protected]

2.7

January 19, 2015

Optimal form MPC reformulation

This kind of optimization problem is commonly known as a ”quadratic programming”, or ”QP”-problem, which according to [6] is ”...the problem of optimizing (minimizing or maximizing) a quadratic function of several variables subject to linear constraints on these variables.” This is a very grateful problem to solve, due to the fact that for a convex problem, if we manage to find a local extremum it is also guaranteed to be a global extremum. Thus a gradient method is an effective way to solve this, since there is only one min/max point. A quadratic function is exactly what we have in the term ∆U T H∆U and all of our constraints are linearised since we use the small angle approximation on our non-linear terms. The standard form of a problem such as this with symmetric constraints, however, is min x

1 T x Hx + fT x, 2

subject to − b ≤ Mx ≤ b

(50)

and to get there requires quite a bit of reformulation. We start remembering that y(k) is the output vector at time tk and then define the following vectors containing the current and all the predicted variables. Starting with y   y(k+1)  y(k+2)    . (51) Y=  ..   . y(k+Np )

(Np ·Ny )×1

Expanding the reference function yref for all time steps as well then becomes:     1 yref 1 yref       1  Yref =   , (52) ⊗ yref = yref   ..   ..  .  .  1

yref

Np ×1

(Np ·Ny )×1

where ⊗ is the Kronecker product. This produces a vector where yref occurs Np times. The Kronecker product is a very handy way of composing vectors where we want to stack several instances of the same element in a new vector or matrix and this technique will be used repeatedly. On a more subtle level, this means that we make the assumption that the reference function is held constant through the prediction horizon. We continue with u as   u(k)  u(k+1)      (53) U =  u(k+2)    ..   . u(k+Nc -1)

17

(Nc ·Nu )×1

Jens Walker [email protected]

January 19, 2015

and lastly we define ∆U:  u(k) − u(k-1)  u(k+1) − u(k)   u(k+2) − u(k+1) ∆U =   ..  .





∆u(k) ∆u(k+1) ∆u(k+2) .. .



          =       ∆u(k+Nc ) (N u(k+Nc ) − u(k+Nc -1)

.

(54)

c ·Nu )×1

Now the next step is to express our problem in terms of ∆U, since this is the variable we want to minimize over. In order to do this, we need to define an augmented state as   x(k) ξ(k) = (55) u(k-1) (N +N )×1 x

u

and with this we redefine the A, B and C matrices as   A B Aξ = , 0Nu xNx INu xNu

 Bξ =

B

 ,

INu xNu

 Cξ = C

(56)

0Ny xNu



(57)

,

(58)

which then updates our system from (15) to the following ξ(k+1) = Aξ ξ(k) + Bξ ∆u yξ (k) = Cξ ξ(i)

(59)

.

According to Wang [4], Y and ∆U can be rewritten on the form Y = Fξ(k) + Φ∆U ,

(60)

where     F=  

Cξ Aξ Cξ A2ξ Cξ A3ξ .. . N

      

Cξ Aξ p

(61)

(Ny ·Np )×Nx

and Φ is the Toeplitz lower-triangular block matrix defined by     Φ=  

Cξ Bξ Cξ Aξ Bξ Cξ A2ξ Bξ .. . N −1

Cξ Aξ p

Bξ

0Ny ×Nu Cξ Bξ Cξ Aξ Bξ .. . N −2

Cξ Aξ p

Bξ

0Ny ×Nu 0Ny ×Nu Cξ Bξ .. .

··· ··· ··· .. .

N −3

···

Cξ Aξ p 18

Bξ

0Ny ×Nu 0Ny ×Nu 0Ny ×Nu .. . N −Nc

Cξ Aξ p

     . (62)   Bξ

Jens Walker [email protected]

January 19, 2015

We need to specify a couple more things in order to redefine our cost function as a function of only ∆U, which is called the optimal formulation. We start off with U, U(k) = κ∆U(k) + U(k-1) , where the lower triangular matrix κ is defined as    1 0 1 0 ··· 0 0 1 1 1 · · · 0    κ = . . . ⊗ . . . . ...   .. ..  .. ..  0 0 1 1 · · · 1 N ×N c

c

(63)

··· ··· .. .

 0 0  ..  .

···

1

,

Nu ×Nu

which results in a (Nu · Nc ) × (Nu · Nc ) matrix. We then specify Uk as     u(k-1) 1 u(k-1) 1       1 . ⊗ u(k-1) = u(k-1) U(k-1) =    ..   ..   .  . 1

u(k-1)

Nc ×1

(64)

(65)

(Nu ·Nc )×1

For brevity, we also rewrite the previous control vector values as Up = U(k-1) ,

(66)

E = Fξ − Yref ,

(67)

as well as where E denotes the error of the system. This allows us to formulate our problem as an optimized form minimization problem in order to find the best control sequence u by minimizing a cost function J that obeys certain constraints. The cost function is mentioned at the start of this section and originally specified as: min, J(k) = ||Y(k) − Yref (k)||2Q + ||U(k)||2S + ||∆U(k)||2R ,

(68)

here the subscripts Q, S, R denote the weight matrices with which we multiply the terms of the cost function in order to reach a satisfactory result. How large the elements of these matrices are decide how much consideration we give the constraints versus how hard we try to match the output to the reference function. These matrices also need to be adapted to Y, U and ∆U so for Q we assemble Np matrices Qm of size Ny ×Ny in a diagonal block matrix which will then look as follows:   Qm 0Ny ×Ny 0Ny ×Ny · · · 0Ny ×Ny 0Ny ×Ny Qm 0Ny ×Ny · · · 0Ny ×Ny    0Ny ×Ny 0Ny ×Ny Qm · · · 0Ny ×Ny  Q= , (69)    .. .. .. .. . .   . . . . . 0Ny ×Ny

0Ny ×Ny

0Ny ×Ny

···

19

Qm

(Np ·Ny )×(Np ·Ny )

Jens Walker [email protected]

January 19, 2015

where each value in Qm directly corresponds to the weight on each of the parameters in y as specified in (26). In the same way we specify S and R, where both Sm and Rm are of size Nu ×Nu and inserted Nc times on the diagonal, which gives   Sm 0Nu ×Nu 0Nu ×Nu · · · 0Nu ×Nu 0Nu ×Nu Sm 0Nu ×Nu · · · 0Nu ×Nu    0Nu ×Nu 0Nu ×Nu Sm · · · 0Nu ×Nu  (70) S=    .. .. .. .. ..   . . . . . 0Nu ×Nu 0Nu ×Nu 0Nu ×Nu · · · Sm (N ·N )×(N ·N ) c

and lastly 

0Nu ×Nu   R = 0Nu ×Nu  ..  .

0Nu ×Nu Rm 0Nu ×Nu .. .

0Nu ×Nu 0Nu ×Nu Rm .. .

··· ··· ··· .. .

0Nu ×Nu

0Nu ×Nu

0Nu ×Nu

···

Rm

u

c

u

 0Nu ×Nu 0Nu ×Nu   0Nu ×Nu    ..  . Rm

.

(71)

(Nc ·Nu )×(Nc ·Nu )

This is one of the parameters of the model that can be tuned and the final values used for Qm , Sm and Rm can be found in Appendix A. We are now ready to rewrite our cost function found in (68) using (51)–(67), so that we get: J(k) = ET QE + UTp SUp + 2 (ET QΦ + UTp Sκ) ∆U + ∆U T (ΦT QΦ + R + κT Sκ) ∆U . {z } | | {z }

(72)

H

f

So now we have found the Hessian matrix H and the gradient vector f to use in (50), so that takes care of the cost function J, but we still need to define our constraint matrices M and b in terms of ∆U . In order to do this, we first need to reformulate our constraints to suit Y, U and ∆U. Suppose that the maximum constraints for each variable in the output vector is expressed as a vector ymax where each element is a constraint for the corresponding variable in y  max  y1 y2max   max    ymax = y3  . (73)  ..   .  max yN y Ny ×1

20

Jens Walker [email protected]

January 19, 2015

Then we define our new constraint vector  max    y 1 ymax  1  max        ⊗ ymax = y Ymax = 1   ..   ..   .  . 1

ymax

Np ×1

,

(74)

(Ny ·Np )×1

which is the constraints stacked Np times to affect every time step of the prediction horizon. Similarly for U, where umax is a vector that contains the maximum allowed values of u for the control horizon. We use the Kronecker product to define    max  1 u 1 umax     max      . (75) ⊗ umax = u Umax = 1   ..   ..   .  . 1

umax

Nc ×1

(Nu ·Nc )×1

And in the same manner for ∆U, where ∆umax is specified as a vector which contains the maximum values of ∆u we specify     ∆umax 1 ∆umax  1     max   1 max max . (76) ⊗ ∆u = ∆u ∆U =      ..  ..   . . 1

∆umax

Nc ×1

(Nu ·Nc )×1

For the minimum constraints, we simply repeat (73)–(76) but replace ymax , umax and ∆ umax with ymin , umin and ∆ umin . Now we can reformulate our problem on the standard form, where our matrix M becomes   κ M= (77) Φ while the constraint vector b is rewritten as the following  Umax − Uk b= . Ymax − Fξ(k) 

(78)

Here we can note that M and b are actually system constant which means that they are time-independent and only need to be calculated once. This is actually true for several matrices and vectors of the system which can be exploited in order to conserve computational resources. In table 1, section 3.2 we list all of the matrices and classify which are constant and which are time-dependent. With this we now have everything in order to be able to apply the MPC model to our system!

21

Jens Walker [email protected]

3

January 19, 2015

Method

In this section we start by looking at how the motion of the platform is handled today and establish inputs and outputs from this. We then move on to describe how we construct the code in order to solve our QP-problem, including how we go about calibrating the model. Lastly we go into a bit more detail what data we wish to extract from the simulated environment.

3.1

Analysis of current implementation

Looking at the current source code is a necessity, mainly in order to know how to adapt the new model to the rest of the system. In addition to discovering what input and output we need to adapt the MPC model to, it gives us a hint of how the platform currently handles the mapping of the degrees of freedom. We have established the state parameters in (20) and all of them can be retrieved from the coordinate system representing the user inside the simulator. It gives us the position as well as the orientation of the user. Furthermore we can find both linear and angular velocities and accelerations by tracking the change in these values between time steps. This means that we need to know how much time has passed between each update, or frame. Thankfully the frame rate is fixed at 60Hz due to adaptations to the displays, which gives us the time step as the reciprocal dt =

3.2

1 . 60

(79)

Constructing code for evaluation

This model, as mentioned in section 2.7 ultimately takes the shape of a QPproblem. Thankfully, there are a lot of software packages that handle this problem since it is a fairly common one. We will solve this problem in Matlab due to its ease of use when it comes to matrices and plotting results. To make the evaluation of data as simple as possible, the extracted simulator data is imported into a Matlab script and solved in ”faux real-time”. That is, instead of reading the state continuously from the platform it is read from a file containing all of the values extracted from a simulation scenario. This is done in a loop over time steps using only the current value as the state and as such the implementation is not aware that the future values are already known. Thus we emulate the same situation that would occur in a real scenario and makes the Matlab script directly portable to another programming language if one so desires. The problem is reformulated according to the theory section of this report and solved with the quadprog() routine found in the Matlab Optimization Toolbox. This routine however wants both the upper and lower constraints in a single vector according to M0 ≤ b0 22

(80)

Jens Walker [email protected]

January 19, 2015

so we reformulate b slightly and stack the constraints, leaving us with:   Umax − Uk  −Umax + Uk   b0 =   Ymax − Fξ(k)  , −Ymax + Fξ(k)

(81)

and adapt M to fit this, which gives us 

 κ  −κ   M0 =  Φ . −Φ

(82)

We have already mentioned that some of the matrices are system constant matrices, which means that they can be calculated once outside the time loop in order to preserve resources. Table 1 shows an overview of which matrices are constant and which are time-dependent. Table 1: Matrices and vectors in the system and their time dependence

System constant Ymax /Ymin Umax /Umin ∆U max /∆U min Q S R H M F Φ κ

Time dependent Y U ∆U ξ b E Yref Uk f

Note that some of these matrices are products of others, but we list all of them to simplify any usage of these matrices for a potential port to another programming language. While for example Q, S and R are only used to construct H, one might want to adjust the values of the weight matrices after migrating the model and thus deciding to calculate H rather than use the already assembled matrix from the program. Since we want the position of the platform to return to it’s neutral position in order to utilize as much of the space as possible, we remember the output vector from (26)  T y = θ˙ φ˙ ψ˙ ax ay az θ φ ψ px vx py vy pz vz . (83) and we use the following reference vector  yref = θ˙ φ˙ ψ˙ ax ay az 0 0 23

0

0

0

0

0

0

0

T

.

(84)

Jens Walker [email protected]

January 19, 2015

In this way every time step we minimize the difference between 0 and the variables θ, φ, ψ, px , vx , py , vy , pz , vz , which ensures that these parameters strive towards zero. This ensures that the platform is automatically return to its neutral position and is mentioned in section 2.5.2 as a solution to the washout-problem. We then go through the time loop, calculating the control and output vectors using the current augmented state ξ and store these for analysis. These are plotted and shown in section 4.2 of the results.

3.3

Calibrating the MPC model

In order to evaluate the performance of our MPC model, we need to adapt the coefficients of our theoretical model to the specific system. This is normally done in two steps by performing both offline and online calibration to tune the model. We will however only do the first and leave the online calibration until the model has been fully implemented. Adapting the model to our platform means that we need to insert the constraints for the output, input and input difference as well as experimentally define what is a suitable prediction and control horizon. The constraints can be found in the documentation for the platform and is shown below in (89)–(91). Note that the constraints are given in degrees and degrees per second since the specifications for the platform follow this standard. These need to be converted to radians and radians per second before any calculations are performed. The offline calibration is performed by extracting data from a predefined scenario and changing the parameters in order to obtain the desired performance. These parameters include the weight matrices Q, S and R that control how much we take the constraints into consideration for U, ∆U and Y respectively as well as the control and prediction horizons. These need to be balanced, since if we constrain the output to harshly we may end up with very brusque movements of the motors which may damage the actuators and platform. However if we weigh the output to lightly, we may end up with a situation where the movement is not tracked well enough. Both the horizons and the weights require some experimenting where we vary the parameters and select appropriate values to get the wanted results. With some influence from [7] and a bit of experimenting we find values that work with our specific problem and arrive at the values listed in Appendix A The online calibration on the other hand is based less on comparing data and more on gut feeling. This step in the process is performed last by driving the simulator and adjusting the weights so as to get ’the correct feel’. It is a matter of calibrating the platform to pick up on all the small subtleties that are hard to model mathematically by looking at plots of data. This type of calibration is crucial for the final product, however is less important when designing and verifying the model. It is thus outside the scope of this project and suggested as a future expansion.

24

Jens Walker [email protected]

3.4

January 19, 2015

Extracting data from the simulator

We need the data from the simulator in order to compose our state and reference vectors and thus need to extract this somehow. Thankfully, all of this is already used for calculations within the simulator and readily available to be printed to a file along with the velocities and accelerations, both linear and angular. In section 4.2 we explore three scenarios for the simulator that are of interest and extract everything we need for the model. ˆ A planar, circular track

Figure 6: Vehicle moving along circular track, path marked as a red line.

ˆ Extreme tilt of the vehicle

Figure 7: Vehicle is forcefully tilted to an extreme angle.

25

Jens Walker [email protected]

January 19, 2015

ˆ Banging the scoop into the ground

Figure 8: The scoop in the air and after it has been slammed into the ground with force.

26

Jens Walker [email protected]

4

January 19, 2015

Experiments and results

We have two results to show, one is for the platform double-pendulum model and the other is for the actual MPC-model.

4.1

Validating platform model

As mentioned in 2.2, we model the motion platform using a constrained double pendulum. In order to verify this model we calculate the vertical displacement of the platform as a function of the motor angle both for the theoretical model as well as for the actual platform. To find the latter, we perform a full roll movement and measure the displacement of the lower and upper ball joints and center these values around the middle. These values are then plotted in figure 9 together with the values of the theoretical model described in (1)–(4). The measured values can be found in Appendix B. Vertical displacement of platform vs motor angle 0.15 Measured values Model values

Vertical displacement of platform (m)

0.1

0.05

0

−0.05

−0.1

−0.15

−0.2 −50

−40

−30

−20

−10 0 10 Angle of motor (deg)

20

30

40

50

Figure 9: Modelled and measured values of displacement vs motor angle.

Analysing the plots in figure 9 we sense a strong linear relationship, which is to be expected since the angles are small. This allows us to perform a linear regression on these two functions to get the equations: y = 0.0030958x − 0.0008779

(85)

y = 0.0030776x + 0.0019765.

(86)

Equation (85) shows the linear regression for the measured values and (86) for the model values. We will discuss the discrepancies between the values in section 4.3.

27

Jens Walker [email protected]

January 19, 2015

In order to be able to implement the model directly into the motor controller, which may be of importance later on, we also want to find the relationship between motor angle and platform angle. We find that the motor angle is linearly related to the pitch and roll and by performing linear regression. We then get the results seen in figures 10–11 below. M otor angle α1 (degrees) vs platform roll angle θ (degrees) 12 Motor angle vs roll Linear fit: 0.37401x + 0.14334 10

θ (degrees)

8

6

4

2

0

0

5

10

15

20

25

30

α1 (degrees)

Figure 10: Modelled values of motor angles vs roll angle.

M otor angle α1 (degrees) vs platform pitch angle φ (degrees) 14 Motor angle vs pitch Linear fit: 0.48254x + 0.37766 12

φ (degrees)

10

8

6

4

2

0

0

5

10

15

20

25

30

α1 (degrees)

Figure 11: Modelled values of motor angles vs pitch angle.

Extracting the coefficients from this to be used in an implementation we get croll = 0.375 and cpitch = 0.483.

28

Jens Walker [email protected]

4.2

January 19, 2015

Validating the MPC model

In order to validate the MPC model we need to keep three things in mind; that the movement of the platform is tracked well, that the constraints are not violated and that the tilt coordination is present in the resulting angles. We plot the raw data from the simulator, denoted simdata alongside the MPCprocessed data, labelled mpc as well as the old model, denoted washout. There are nine parameters of interest that we use to analyse the performance of our ˙ φ, ˙ ψ, ˙ ax , ay and az . model, which are θ, φ, ψ, θ, For a quantitative comparison of the methods we use the l2 -norm. More precisely we take the difference of the l2 -norm for the raw data and the methods respectively and denote them 2 lwo = | ||iwo ||2 − ||iraw ||2 | ,

(87)

2 2 2 lM P C = | ||iM P C || − ||iraw || | ,

where i signifies one of the nine component. This is then shown in a table and plotted in bar graphs for each scenario. All of the output vectors y, x, u and ∆u are plotted in appendix C for the three scenarios. Below we focus on the most interesting of these plots and analyse these. 4.2.1

Tilt coordination evaluation

For the tilt coordination, we first recollect the principle from section 2.3. Sustained accelerations in the lateral plane is represented as an angle by utilizing ˆ in figure the acceleration of gravity. We then compare the acceleration in x 12 with the roll angle θ in figure 13 and can see that the accelerations indeed influence the angle as one would want and expect. Linear accelerations for circular path Acceleration in x 10

simdata mpc

ax (m/s2 )

5

0

−5

−10

−15

0

0.2

0.4

0.6

0.8 1 time (s)

1.2

1.4

1.6

1.8

Figure 12: The linear acceleration in x-direction.

29

Jens Walker [email protected]

January 19, 2015

Angles for circular path Roll 0.06

simdata mpc

0.05

0.04

θ (rad)

0.03

0.02

0.01

0

−0.01

−0.02

0

0.2

0.4

0.6

0.8 1 time (s)

1.2

1.4

1.6

1.8

Figure 13: The roll angle with tilt coordination showing.

4.2.2

Constraint handling evaluation

As stated in (89) the maximum allowed pitch is 10 degree which translated to radians is about 0.175 radians and as we can see in figure 14 this is well exceeded during the extreme tilt maneuver. This means that the model respects the constraints and limits the platform from exceeding the pre-set value, something that the old washout model does not, which requires manual limiting of the angles in a separate step. Looking at figure 15, the MPC model automatically adjusts the corresponding angular velocity while the washout use the same angular velocity as without the limiting which could possibly create a mismatch in sensation. So not only does the MPC model not require any additional work, it also incorporates the limitations of the constraints into its behaviour for the other parameters. Here we see confirmation that the platform indeed can utilize its full range.

30

Jens Walker [email protected]

January 19, 2015

Angles for extreme tilt manouver Pitch 0.2

simdata mpc washout

0.19

φ (rad)

0.18

0.17

0.16

0.15

30

30.2

30.4

30.6

30.8

31 31.2 time (s)

31.4

31.6

31.8

32

Figure 14: Pitch angle during extreme tilt maneuver.

Angular velocities for extreme tilt manouver Pitch 0.15

simdata mpc washout

0.1

0.05

φ˙ (rad/s)

0

−0.05

−0.1

−0.15

−0.2

−0.25

−0.3 30

30.2

30.4

30.6

30.8

31 31.2 time (s)

31.4

31.6

31.8

32

Figure 15: Pitch velocity during extreme tilt maneuver.

31

Jens Walker [email protected]

4.2.3

January 19, 2015

Accuracy evaluation

If the values of our model and the raw data coincide well, we know that the model is accurate and we do so by looking at the difference in l2 -norm between the model output and the raw simulation data. Although looking at the difference in the l2 -norm is in no way a perfect measure of the performance of the model since sometimes we do not want the output from our model to completely match the raw data, for example when the tilt coordination influences the angle or when a parameter is restricted by the constraints, it still gives an indication of how well the parameters are tracked. We look at the bar plots of these tables depicted in figures 16, 17 and 18. Looking at these bar plots, we see that overall the parameters are tracked very well with a typical discrepancy in the l2 -norm of 10−1 .

3

Logarithmic bar plot of the absolute difference in l2−norm

10

MPC−simdata washout−simdata

2

Logarithm of the discrepancy

10

1

10

0

10

−1

10

−2

10

−3

10

1

2

3

4

5 6 Parameter

7

8

9

Figure 16: Bar graph of the absolute difference in l2 norm.

32

Jens Walker [email protected]

2

January 19, 2015

Logarithmic bar plot of the absolute difference in l2−norm

10

MPC−simdata washout−simdata

1

Logarithm of the discrepancy

10

0

10

−1

10

−2

10

−3

10

−4

10

1

2

3

4

5 6 Parameter

7

8

9

Figure 17: Bar graph of the absolute difference in l2 norm.

2

Logarithmic bar plot of the absolute difference in l2−norm

10

MPC−simdata washout−simdata

1

10

0

Logarithm of the discrepancy

10

−1

10

−2

10

−3

10

−4

10

−5

10

1

2

3

4

5 6 Parameter

7

8

9

Figure 18: Bar graph of the absolute difference in l2 -norm.

33

Jens Walker [email protected]

4.3

January 19, 2015

Sources of error

The offset could be due to a systematic error in the measurements such as a parallax error. This is always a risk when using a manual measuring device, in this case a measuring tape. It could also be due to hysteresis in the ball joints, that is that the middle of the ball joint can be moved slightly and not be completely aligned with the motor axis vertically in its rest position. This would also account for it not being centered around zero. One last possibility is that the rest position of the lower ball joint is not completely aligned vertically with the motor axis. Since a difference in platform displacement of anything below 10−3 m gives a negligible impact on the pitch/roll angle and is in most cases indistinguishable to the driver of the platform. Further the hysteresis of the ball joints themselves account for errors larger than that of the numerical model. Thus we can neglect the error of the mathematical model, since the error is of order 10−5 m.

4.4

Possible expansions

The expansions listed below are the next steps in making the model better, faster and more accurate. 4.4.1

Implementation and online calibration

The first expansion is to port the solution to the required format and implement on the current hardware. When this has been done one can perform the online calibration described earlier by tweaking weights, horizons and constraints. In order to speed along this process the current Matlab code exports all the matrices to a comma-separated .txt-file to be imported into any ported solution. 4.4.2

Convert to a sparse system

Most of the matrices are fairly large with up to 105 elements depending on the parameter size and many of them are very sparse with only elements on the diagonal. It would then make sense to convert this to a sparse system and preserve both memory and possibly speed up the calculations as well. 4.4.3

Vehicle model

One further step is to implement a more complex vehicle model, that takes into consideration suspension, steering angle and similar parameters. This would enable even further customisation to the current application.

34

Jens Walker [email protected]

5

January 19, 2015

Conclusions

The time has come to tie the results to the goals in section 1.3, so we look at the goals one by one.

5.1

Precision of the model

”The goal of this project is to develop a mathematical model that accurately reflects the angles, velocities and accelerations of the vehicle while simultaneously taking into account the limited range of the mpf.” As we could see in section 4 all of the parameters that we are interested in are tracked very well, even while respecting the constraints, which we could see clearly in figures 14–15. The l2 -norm shows that the MPC model performs a lot better than the washout model that is currently used.

5.2

Tilt coordination

”Another key aspect of the model is that tilt coordination should be present.” We also concluded that tilt coordination was indeed present from figure 13, so the outcome with regards to this goal can be said to be positive.

5.3

Numerical feasibility

”The model should be optimized for numerical solving and a prototype code should be constructed.” Since the MPC model allows us to construct a QP-problem which is very well suited for numerical solving, we may conclude that the first part of this goal is fulfilled. As for the second part, the code is constructed in Matlab which allows for easy plotting and simplicity when handling matrices. This prototype can be ported to the required programming language and at the same time be used to verify the ported solution.

35

Jens Walker [email protected]

A

January 19, 2015

Appendix - Coefficients used Table 2: Vestibular system transfer function coefficients.

Coefficient

i=ˆ x

i=ˆ y

i=ˆ z

Til

6.1

5.3

10.2

Tis

0.1

0.1

0.1

Tia

30

30

30

τli

5.33

5.33

5.33

τsi

0.66

0.66

0.66

τai

13.2

13.2

13.2

0.4

0.4

0.4

K

i

Np = 15 Nc = 10

(88)

Below are the weights listed together with the constraints and the vectors y, u and ∆u. These are all found in 26 to easily see which parameters carry which weight and has which constraint. Below in the diagonal matrix (89) we see the weights Qm as well as the constraints for y h iT y = θ˙ φ˙ ψ˙ ax ay az θ φ ψ px vx py vy pz vz , h i Qm = diag , 2000 2000 300 60 30 10 60 60 20 20 20 20 20 30 15 h iT ymax = + 500 500 400 150 150 100 10 10 10 100 10 100 10 100 10 , h iT ymin = − 500 500 400 150 150 100 10 10 10 100 10 100 10 100 10 (89) and the weight matrix Sm for u are displayed below in (90) along with the constraints h iT , u = θ˙ φ˙ ψ˙ ax ay az h i Sm = diag , 1 1 1 1 1 1 (90) h iT umax = + 30 30 40 10 10 10 , h iT umin = − 30 30 40 10 10 10

36

Jens Walker [email protected]

January 19, 2015

and the weight matrix Rm for the rate of change in control sequence ∆u as well as the constraints are given as h iT ∆u = ∆θ˙ ∆φ˙ ∆ψ˙ ∆ax ∆ay ∆az . h i Rm = diag 1 1 1 0.1 0.1 0.1 h iT ∆umax = + 150 150 150 100 100 100 h iT ∆umin = − 150 150 150 100 100 100

37

, , (91) , .

Jens Walker [email protected]

B

January 19, 2015

Appendix - Platform measurements

Below in table 3 we see the values measured for the platform during a roll movement. Knowing the length of the lever arm allows us to find the motor angle from the position of the lower joint and the displacement of the platform from the position of the upper. All values are adjusted around the center, where the center is defined as the rest position of the platform when it is parallel to the ground. The angles are found through:   lower ball joint position α1 = sin−1 (92) lever arm length

Table 3: Measured positions of actuator joints during a full roll.

Upper ball joint

Lower ball joint

Resulting angle (°)

-0.124

-0.119

-39.78

-0.111

-0.111

-36.64

-0.096

-0.096

-31.07

-0.079

-0.076

-24.12

-0.058

-0.057

-17.85

-0.041

-0.039

-12.10

-0.021

-0.019

-5.86

0

0

0

0.020

0.021

6.48

0.040

0.039

12.10

0.060

0.060

18.82

0.078

0.079

25.13

0.096

0.096

31.07

0.113

0.114

37.80

0.124

0.121

40.58

38

Jens Walker [email protected]

C

January 19, 2015

Appendix - Complete model output

C.1

Circular path Angles for for circular path Roll

θ (rad)

0.1

simdata mpc washout

0

−0.1

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Pitch

φ (rad)

0.01

simdata mpc washout

0 −0.01 −0.02 −0.03

0

0.2

0.4

0.6

0.8 time (s) Yaw

1

1.2

1.4

1.6

ψ (rad)

1

simdata mpc washout

0.5 0 −0.5

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Figure 19: The angles for the circular path.

Angular velocities for circular path

θ˙ (rad/s)

Roll 0.5

simdata mpc washout

0 −0.5 −1

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Pitch

φ˙ (rad/s)

1

simdata mpc washout

0.5 0 −0.5

0

0.2

0.4

0.6

0.8 time (s) Yaw

1

1.2

1.4

1.6

ψ˙ (rad/s)

0.5

simdata mpc washout

0 −0.5 −1

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Figure 20: The rotational velocities for the circular path.

39

Jens Walker [email protected]

January 19, 2015

Linear accelerations for circular path

ax (m/s2 )

Acceleration in x 10

simdata mpc washout

0 −10 −20

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Acceleration in y

ay (m/s2 )

30

simdata mpc washout

20 10 0 −10

0

0.2

0.4

0.6

0.8 time (s) Acceleration in z

1

1.2

1.4

1.6

az (m/s2 )

0.5

simdata mpc washout

0 −0.5 −1

0

0.2

0.4

0.6

0.8 time (s)

1

1.2

1.4

1.6

Figure 21: The linear accelerations for the circular path.

Table 4: Absolute difference in l2 -norm for each method.

Parameter

l2wo

l2M P C

θ

0.0026

0.0061

φ

0.0025

0.0011

ψ

0.0179

6.1798

θ˙

0.0072

0.7321

φ˙

0.0041

0.1540

ψ˙

0.0185

1.4022

ax

1.1069

49.7561

ay

1.9495

103.5735

az

0.0204

1.1961

40

Jens Walker [email protected]

0.2

0.8

0

0.6

φ˙ (rad/s)

θ˙ (rad/s)

0.4

Control sequence rate values for circular path Pitch velocity 1

−0.2 −0.4 −0.6

0.2 0 −0.2

−1

−0.4

0

1 time (s)

2

0.2

0.4

−0.8

0

1 time (s)

−0.6

−1

2

0

1 time (s)

−4 −6 −8

z−acceleration 0.2

6 4 2 0

2

2

0.25

0.15

az (m/s2 )

8

ay (m/s2 )

ax (m/s2 )

−2

1 time (s)

−0.4

y−acceleration 10

0

0 −0.2

−0.8

x−acceleration 0

−10

Yaw velocity 0.4

ψ˙ (rad/s)

Roll velocity

January 19, 2015

0.1 0.05 0 −0.05

0

1 time (s)

−0.1

2

0

1 time (s)

2

Figure 22: Control sequence for the circular path.

Control sequence values for circular path Roll acceleration Pitch acceleration 1

0.5

Yaw acceleration 0.4

0

−0.5

0.2

∆ψ˙ (rad/s2 )

∆φ˙ (rad/s2 )

∆θ˙ (rad/s2 )

0.8 0.6 0.4 0.2 0 −0.2 −1

0

1 time (s)

−0.4

2

−0.4 −0.6 −0.8

0

1 time (s)

−1

2

y−acceleration rate 0.15

0

0.5

0.1

−1 −1.5 −2

0

1 time (s)

2

∆az (m/s3 )

1

−0.5

0 −0.5 −1 −1.5

0

1 time (s)

2

0

1 time (s)

0.05 0 −0.05 −0.1

0

1 time (s)

Figure 23: Control sequence rate for the circular path.

41

2

z−acceleration rate

0.5

∆ay (m/s3 )

∆ax (m/s3 )

x−acceleration rate

0 −0.2

2

Jens Walker [email protected]

C.2

January 19, 2015

Extreme tilt Angles for extreme tilt manouver Roll

θ (rad)

−0.04

simdata mpc washout

−0.06 −0.08 −0.1 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Pitch

φ (rad)

0.2

simdata mpc washout

0.18 0.16

30

30.2

30.4

30.6

30.8

31 time (s) Yaw

31.2

31.4

31.6

31.8

32

ψ (rad)

0.5

simdata mpc washout

0 −0.5 −1 −1.5 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Figure 24: The angles for the extreme tilt manouver.

Angular velocities for extreme tilt manouver

θ˙ (rad/s)

Roll 0.2

simdata mpc washout

0

−0.2 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Pitch

φ˙ (rad/s)

0.4

simdata mpc washout

0.2 0 −0.2 −0.4 30

30.2

30.4

30.6

30.8

31 time (s) Yaw

31.2

31.4

31.6

31.8

32

ψ˙ (rad/s)

0.3

simdata mpc washout

0.2 0.1 0 −0.1 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Figure 25: The rotational velocities for the extreme tilt manouver.

42

Jens Walker [email protected]

January 19, 2015

Linear accelerations for extreme tilt manouver

ax (m/s2 )

Acceleration in x 5

simdata mpc washout

0

−5 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Acceleration in y

ay (m/s2 )

20

simdata mpc washout

10 0 −10 −20 30

30.2

30.4

30.6

30.8

31 31.2 time (s) Acceleration in z

31.4

31.6

31.8

32

az (m/s2 )

10

simdata mpc washout

5 0 −5 30

30.2

30.4

30.6

30.8

31 time (s)

31.2

31.4

31.6

31.8

32

Figure 26: The linear accelerations for the extreme tilt manouver. Table 5: Absolute difference in l2 -norm for each method.

Parameter

l2wo

l2M P C

θ

0.0007

0.0000

φ

0.0037

0.0000

ψ

0.0001

0.1845

θ˙

0.0005

0.0201

φ˙

0.0137

0.0032

ψ˙

0.0009

0.0145

ax

0.4013

1.4631

ay

0.3051

26.8611

az

0.0967

7.3437

43

Jens Walker [email protected]

January 19, 2015

Control sequence rate values for extreme tilt manouver Roll velocity Pitch velocity 0.5 0.45 0

0.4

−0.5

0.35

φ˙ (rad/s)

θ˙ (rad/s)

0.1 0.05 0 −0.05

ψ˙ (rad/s)

0.15

−1 −1.5 −2 −2.5

−0.1 30

31 time (s)

31 time (s)

x−acceleration

0.2

0.1 30

32

−9

6

−9.2 −9.4 −9.6

z−acceleration −2

4 2

−2.2 −2.4 −2.6 −2.8

0

−9.8 31 time (s)

−3

−2 30

32

32

−1.8

az (m/s2 )

8

−10 30

31 time (s)

y−acceleration

−8.8

ay (m/s2 )

ax (m/s2 )

0.3 0.25

0.15

−3 30

32

Yaw velocity

31 time (s)

32

−3.2 30

31 time (s)

32

Figure 27: Control sequence during extreme tilt maneuver.

0.1

Control sequence values for extreme tilt manouver Roll acceleration Pitch acceleration Yaw acceleration 0.15 0.15

0

−0.05

∆ψ˙ (rad/s2 )

∆φ˙ (rad/s2 )

∆θ˙ (rad/s2 )

0.1 0.05

0.05 0 −0.05 −0.1

0.1

0.05

0

−0.15 31 time (s)

−0.2 30

32

31 time (s)

32

y−acceleration rate 6

0

4

−2 −4 −6 −8 −10 30

31 time (s)

32

31 time (s)

32

z−acceleration rate 1 0.5

2 0 −2 −4 30

−0.05 30

∆az (m/s3 )

∆ay (m/s )

x−acceleration rate 2

3

3

∆ax (m/s )

−0.1 30

0 −0.5 −1 −1.5 −2

31 time (s)

32

−2.5 30

31 time (s)

32

Figure 28: Control sequence rate during extreme tilt maneuver.

44

Jens Walker [email protected]

C.3

January 19, 2015

Scoop slam Angles for scoop slam −3

θ (rad)

2

Roll

x 10

simdata mpc washout

0 −2 −4 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Pitch

φ (rad)

0.025

simdata mpc washout

0.02 0.015 0.01 4.8

4.9

5

5.1

5.2

5.3 time (s) Yaw

5.4

5.5

5.6

5.7

5.8

ψ (rad)

0.02

simdata mpc washout

0.01 0 −0.01 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Figure 29: The angles for the scoop slam.

Angular velocities for scoop slam

θ˙ (rad/s)

Roll 0.1

simdata mpc washout

0

−0.1 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Pitch

φ˙ (rad/s)

0.02

simdata mpc washout

0.01 0 −0.01 4.8

4.9

5

5.1

5.2

5.3 time (s) Yaw

5.4

5.5

5.6

5.7

5.8

ψ˙ (rad/s)

0.02

simdata mpc washout

0.01 0 −0.01 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Figure 30: The rotational velocities for the scoop slam.

45

Jens Walker [email protected]

January 19, 2015

Linear accelerations for scoop slam

ax (m/s2 )

Acceleration in x 1

simdata mpc washout

0

−1 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Acceleration in y

ay (m/s2 )

10

simdata mpc washout

5 0 −5 −10 4.8

4.9

5

5.1

5.2

5.3 5.4 time (s) Acceleration in z

5.5

5.6

5.7

5.8

az (m/s2 )

2

simdata mpc washout

0 −2 −4 −6 4.8

4.9

5

5.1

5.2

5.3 time (s)

5.4

5.5

5.6

5.7

5.8

Figure 31: The linear accelerations for the scoop slam. Table 6: Absolute difference in l2 -norm for each method.

Parameter

l2wo

l2M P C

θ

0.0002

0.0281

φ

0.1500

0.0052

ψ

0.0335

17.1242

θ˙

0.0166

0.1829

φ˙

1.3337

0.2205

ψ˙

0.1316

1.6575

ax

0.1539

14.4211

ay

0.5521

47.8782

az

0.1474

11.3545

46

Jens Walker [email protected]

Roll velocity 0.08

January 19, 2015

Control sequence rate values for scoop slam Pitch velocity 0.055

0.02 0 −0.02

0.045 0.04 0.035

0.005

0

−0.005

0.03

−0.04 −0.06

ψ˙ (rad/s)

0.05

0.04

φ˙ (rad/s)

θ˙ (rad/s)

0.06

Yaw velocity 0.01

4

5 time (s)

6

0.025

4

x−acceleration

5 time (s)

6

−0.01

6

z−acceleration

6

−2 −2.2

−9 −9.2 −9.4 −9.6

az (m/s2 )

4

ay (m/s2 )

ax (m/s2 )

5 time (s)

y−acceleration

−8.8

2

0

−9.8 −10

4

−2.4 −2.6 −2.8 −3 −3.2

4

5 time (s)

−2

6

4

5 time (s)

6

−3.4

4

5 time (s)

6

Control sequence values for scoop slam Roll acceleration Pitch acceleration 0.08 0.04

6

0.06

4

0.02 0

0.02 0.01 0

−0.02

−0.01

−0.04

−0.02

4

5 time (s)

6

0

2

−2 −4 −6

−10

−6

6

5 time (s)

0 −2 −4

−8

6

4

5 time (s)

6

z−acceleration rate 1 0.5

−2 −4

5 time (s)

4

0

−8 4

2

y−acceleration rate 4

∆ay (m/s3 )

∆ax (m/s3 )

x−acceleration rate 2

−3 xYaw 10 acceleration

−6

∆az (m/s3 )

0.04

∆ψ˙ (rad/s2 )

0.03

∆φ˙ (rad/s2 )

∆θ˙ (rad/s2 )

Figure 32: Control sequence for the scoop slam.

0 −0.5 −1 −1.5 −2

4

5 time (s)

6

−2.5

4

5 time (s)

Figure 33: Control sequence rate for the scoop slam.

47

6

References [1] Prodyut Das Vestibular Rehabilitation Exercise 2014 http://www.physiotherapy-treatment.com/Vestibular-RehabilitationExercise.html [2] Chin-I Huang, Human Visual-Vestibular Based (HVVB) Adaptive Washout Filter Design for VR-based Motion Simulator. National Kaohhsiung First University of Science and Technology, Taiwan, 2010 [3] Sensory Systems/Vestibular System Simulation 2014 http://en.wikibooks.org/wiki/Sensory Systems/Vestibular System Simulation [06 dec 2014] [4] Chin-I Huang, Human Visual-Vestibular Based (HVVB) Adaptive Washout Filter Design for VR-based Motion Simulator. National Kaohhsiung First University of Science and Technology, Taiwan, 2010 [5] Wikipedia State-space representation 2014 http://en.wikipedia.org/wiki/State-space representation [20 nov 2014] [6] Wikipedia Quadratic programming 2014 http://en.wikipedia.org/wiki/Quadratic programming [29 dec 2014] [7] Bruno Daniel Correia Augusto, Rui Jos´e Leal Loureiro Motion Cueing in the Chalmers Driving Simulator: A Model Predictive Control Approach. Chalmers University of Technology, G¨oteborg, Sweden, 2009 [8] L.D Reid, M.A Nahon Flight Simulation Motion-base Drive Algorithms: Part 1 - Developing and Testing the Equations. UTIAS Report No.296, CN, 1985 [9] Wikipedia Motion simulator - Implementation using washout filters 2014 http://en.wikipedia.org/wiki/Motion simulator#Implementation using washout filters [22 dec 2014] [10] C.D Larsen Comparison of Three Defree of Freedom and Six Degree of Freedom Motion Bases Utilizing Classical Washout Algorithms. Iowa State University, USA 2011 [11] Wikipedia Model predictive control 2015 http://en.wikipedia.org/wiki/Model predictive control [12 jan 2015]

48