IN DEGREE PROJECT TECHNOLOGY,FIRST CYCLE, 15 CREDITS

, STOCKHOLM SWEDEN 2020

Self-Stabilizing Platform Självstabiliserande serveringsbricka

MARCUS AKKILA

BIX ERIKSSON

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT

Self-Stabilizing Platform

MARCUS AKKILABIX ERIKSSON

Bachelor’s Thesis at ITMSupervisor: Nihad SubasicExaminer: Nihad Subasic

TRITA-ITM-EX 2020:49

Abstract

This project explores the possibility to stabilize a handheld serving-tray using a micro-controller, two servomo-tors and an inertial measurement unit (IMU). It is heav-ily focused on control theory, specifically using a PID con-troller. Stabilization of an object using a PID controllerhave many applications such as drones, camera stabilizersand flight simulators. The report covers the theory neces-sary to construct a self stabilizing platform and describesinvolving components in the prototype and how they coop-erate. With the gyroscopes and accelerometers involved inthe IMU it is possible to determine orientation and rota-tion of the tray. The construction enables rotation aboutthe x-axis (roll) and y-axis (pitch) but not the z-axis (yaw).The readings from the gyroscopes and the accelerometersare combined and filtered through a complementary filterto estimate the rotations. The servomotors compensate dis-turbance in keeping the platform horizontal through PIDregulation. The PID constants are tuned through tiltingthe platform at a specific angle and plotting the step re-sponse in MATLAB.

Keywords: Mechatronics, PID controller, Control theory,Arduino, Servomotor, Inertial measurement unit, Self sta-bilization

ReferatSjälvstabiliserande serveringsbricka

Detta projekt utforskar mojligheten att stabilisera enhandhallen serveringsbricka med hjalp av en mikrokontrol-ler, tva servomotorer och en troghetssensor (IMU). Pro-jektet lagger mycket fokus pa reglerteknik, specifikt attanvanda en PID-regulator. Stabilisering genom PID-regleringar anvandbart i manga olika produkter, exempelvis dronare,kamerastabiliserare och flygsimulatorer. Rapporten tackerrelevant teori for att konstruera en sjalvstabiliserande platt-form och beskriver ingaende komponenter i prototypen samthur de samverkar. Med gyroskopen och accelerometrarnasom finns i IMU:n ar det mojligt att uppskatta positionoch rotation for ett objekt. Konstruktionen tillater rota-tion kring x-axeln (roll) och y-axeln (pitch) men inte z-axeln (yaw). Matningarna fran gyroskopen och accelero-metrarna kombineras och filtreras med hjalp av ett s.k.complementary filter for att uppskatta rotationen av ob-jektet. Servomotorerna anvands i sin tur till att halla plat-tan horisontell genom att kompensera storningar fran om-givningen. Detta gors genom PID-reglering. Konstanternai PID-regulatorn ar framtagna genom tester dar plattfor-men lutas ett bestamt antal grader och stegsvaret plottas iMATLAB.

Nyckelord: Mekatronik, PID-regulator, Reglerteknik, Ar-duino, Servomotor, Troghetssensor, Sjalvstabilsering

Acknowledgements

We would especially like to thank Seshagopalan Thorapalli Muralidharan for helpingus throughout this project. He has been available regardless of the time of day. Wehad several components that broke close to the deadline and he supplied us with newcomponents within 24 hours. A thank you to Staffan Qvarnstrom for supplying uswith the electronic components. We would also like to thank Nihad Subasic, courseexaminer, for allowing us to challenge us with this project. Lastly we would like tothank our classmates for their helpful feedback on the report.

Contents

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Theory 52.1 Inertial Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . . 52.2 Filtering Disturbances . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2.1 Complemantary Filter . . . . . . . . . . . . . . . . . . . . . . 52.3 Calculating Roll and Pitch Angles Using Accelerometers . . . . . . . 62.4 PID controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.4.1 Servomotors and PWM . . . . . . . . . . . . . . . . . . . . . 92.5 Microcontroller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.6 Mathematical Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Materials and Design 133.1 Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.2 The TowerPro SG-5010 Servomotor . . . . . . . . . . . . . . . . . . . 133.3 The Inertial Measurement Unit LSM6DS3 by SparkFun . . . . . . . 143.4 Electrical Circuit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.5 Controller for the Platform . . . . . . . . . . . . . . . . . . . . . . . 163.6 The Code for the Platform . . . . . . . . . . . . . . . . . . . . . . . 16

4 Results 194.1 PID response without weight distribution . . . . . . . . . . . . . . . 194.2 PID response with weight distribution . . . . . . . . . . . . . . . . . 23

5 Discussion 25

6 Conclusion 27

7 Recommendations for Future Work 29

Bibliography 31

Appendices 32

A Arduino Code 33

B MATLAB Code 39

List of Figures

1.1 A Stewart Platform [2] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Experimental setup, made in Adobe illustrator[4] . . . . . . . . . . . . . 3

2.1 Complementary filter made in paint . . . . . . . . . . . . . . . . . . . . 62.2 Orientation of the serving tray, made in Adobe illustrator[4] . . . . . . . 72.3 Block diagram showing the implementation of a PID-controller in a sys-

tem[13]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.4 Different duty cycles [14] . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.5 Servomotor connected to tray made in Paint . . . . . . . . . . . . . . . 112.6 Tilting tray made in Paint . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.1 Design of the platform made in Solid Edge ST9[15] . . . . . . . . . . . . 133.2 Servomotors used in the prototype[17] . . . . . . . . . . . . . . . . . . . 143.3 Inertial measurement unit used in the prototype[18] . . . . . . . . . . . 143.4 Circuit diagram made in Fritzing[20] . . . . . . . . . . . . . . . . . . . . 153.5 Flowchart over how the code for the platform works, made in draw.io[21] 17

4.1 Kp = 4, Ki = 0, Kd = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . 204.2 Kp = 1.35, Ki = 0.57, Kd = 0 . . . . . . . . . . . . . . . . . . . . . . . . 214.3 Kp = 1.35, Ki = 0.57, Kd = 0.08 . . . . . . . . . . . . . . . . . . . . . . 224.4 250 gram weight Kp = 1.35, Ki = 0.57, Kd = 0.08 . . . . . . . . . . . . 234.5 250 gram weight Kp = 1.5, Ki = 0.57, Kd = 0.04 . . . . . . . . . . . . . 24

List of Tables

3.1 Table over components . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

List of Abbreviations

IMU Inertial Measurement UnitPID Proportional-Integral-DerivativeCAD Computer Aided DesignPWM Pulse Width ModulationCPU Central Processing UnitDC Direct CurrentSPI Serial Peripheral InterfaceI2C Inter-Integrated CircuitFIFO First In First Out

Chapter 1

Introduction

1.1 Background

An inspiration for this project was the advanced platform type called the Stew-art Platform. A Stewart Platform uses six motors to be able to balance objectson it. It can detect position and motion of the object using sensors, and thencompensate the motion by using its motors to make adjustments in six degrees offreedom. Figure 1.1 shows a Stewart platform made by Acrome Robotics. Withgood equipment it can react very fast and with great precision. The first Stew-art Platform was presented in 1965 in form of a flight simulator[1]. It is nowwidely used in industries such as aerospace, nautical and automotive technology.

Figure 1.1. A Stewart Platform [2]

To stabilize an object using control systems is a classic implementation of mecha-tronics. Useful applications include drones, gimbals for cameras and spoons that

1

CHAPTER 1. INTRODUCTION

keep themselves horizontal. A gimbal is a support that allows rotation about anaxis. The 3-axis camera gimbal is a handheld mount for the camera which usesthree motorized gimbals to compensate for any disturbances in keeping the camerastable. Algorithms and sensors are used to calculate how much the motors mustrotate to cancel out any unwanted movement. Self stabilizing spoons can be usedby people with diseases that causes hand tremors such as Parkinson’s disease. Thissimplifies the eating process a lot for these people[3].

1.2 Purpose

The serving tray is to be used in restaurants were drinks are to be served quick,without spilling any of the liquid. The research questions for this project are:

1. How do we use the information from the inertial measurement unit (IMU) tostabilize the platform?

2. How will variations in PID constants affect the performance in regards tospeed regulations and smoothness?

3. How will differences in load and weight distribution affect the PID-controller?

1.3 Method

The prototype is built up by servomotors, a microcontroller of type Arduino Unoand an inertial measurement unit of type LSM6DS3. These are used to keep theserving tray horizontal when walking with it. The servomotors are placed on top ofa base platform perpendicular to each other. These are used to control the angle ofa platform (the serving tray) on top of the base platform. A pole with a ball jointis used in the middle of the platform to support the servomotors.

2

1.4. SCOPE

A PID controller has been implemented to govern the stabilization of the plat-form.

Figure 1.2. Experimental setup, made in Adobe illustrator[4]

The performance of the PID has been analyzed by introducing a step-responseto the system. The step-response in this case consists of tilting the tray from anhorizontal position, which in turn will create an angle offset error, α, as seen infigure 1.2. The PID controller will then try to compensate this disturbance. ThePID response has been registered and plotted using MATLAB[5]. The aim of thetest is to see how the P, I and D parts of the controller work together to stabilizethe platform, and how this relationship change when a weight distribution is added.

1.4 ScopeThe project will involve a prototype and a report. The purpose of this project is toproduce a self-stabilizing platform with simplicity and low cost. The prototype willbe produced with a budget of 1000 SEK. A Stewart platform can move freely withsix degrees of freedom. The platform in this project will be able to rotate aboutthe x-axis and y-axis but not around the z-axis, what is typically called the yawrotation. Necessary theory needed to construct this platform will be covered in thetheory chapter.

3

Chapter 2

Theory

2.1 Inertial Measurement Unit (IMU)

An inertial measurement unit is an electronic sensor that is placed on a body inorder to determine where and how it is moving. The IMU detects linear accelera-tion using accelerometers and rotational rate using gyroscopes. One accelerometer,gyroscope and magnetometer for each vehicle axis is a common configuration. Themagnetometer is used as a heading reference[6]. The IMU allows tracking of abody’s speed, acceleration, turn rate and inclination. It is therefore commonly usedfor navigational purposes. In aircrafts, where it is highly necessary to track changesin attitude, it is an essential component.

2.2 Filtering Disturbances

When using an IMU a problem arises when trying to combine the accelerometer andgyroscope data. Both are used for the same purpose, to get the angular positionof the object. Some data will cause disturbances in the system and therefore thedata should be filtered in order to gather information about the actual state ofthe system. The powerful recursive filter called Kalman filter is useful when theinformation received from a dynamic system is unpredictable. Therefore it mightseem suitable for this application, but it is far too complex. A complementary filteris most likely more appropriate.

2.2.1 Complemantary Filter

A complementary filter is a steady state Kalman filter for a certain class of filteringproblems[7]. When it is applied on the values received from the IMU it evaluatesan estimated angular position by mostly using the information received from thegyroscope, but also some from the accelerometers. The data received from thegyroscope will be valued higher in the estimation because it is more accurate anddoes not suffer from external forces. The downside of the gyroscope is that it drifts,

5

CHAPTER 2. THEORY

meaning that it introduces error in the output as time passes[8]. The complementaryfilter can therefore be implemented to get a more accurate estimation of the rolland pitch angles. It cannot be used to calculate yaw since the accelerometers doesnot give any data about the yaw (the reason for this is explained in chapter 2.3).The yaw can therefore only be estimated using the gyroscope when a six degrees offreedom IMU is used.A low pass filter is used to filter short term accelerometer fluctuations and a highpass filter is used to negate the drift of the gyroscope. Figure 2.1 shows how θ(roll or pitch) is calculated. θgyro is the gyroscopic value and it is a high frequencysignal passing through a low pass filter G(s). θacc is the accelerometer value, a lowfrequency signal passing through a high pass filter 1-G(s).

θacc(1−G(s)) + θgyroG(s) = θ (2.1)

Figure 2.1. Complementary filter made in paint

The theory behind this is that the noise in the high frequency signal is mostly lowfrequency. It should therefore be filtered with a low pass filter G(s)[7]. The oppositeapplies to the low frequency signal.

2.3 Calculating Roll and Pitch Angles UsingAccelerometers

The orientation of the tray can be described by the pitch, roll and yaw angles ofthe platform and it is essential for the project to accurately calculate these angles.The calculation of the angles is done through the accelerometers on the IMU whichmeasure acceleration in the unit g (gravitational acceleration). When measuringorientation in the earth’s gravitational field, the accelerometers should be calibratedso that one of the accelerometers gives the value +1g when an axis is aligned withthe earth’s gravitational field. Let a be the linear acceleration of the object and Gp

be the vector containing the measurements from the accelerometers, then:

Gp =

Gpx

Gpy

Gpz

= R(g − ar) (2.2)

6

2.3. CALCULATING ROLL AND PITCH ANGLES USING ACCELEROMETERS

Figure 2.2. Orientation of the serving tray, made in Adobe illustrator[4]

where R is the rotation matrix describing the orientation of the object relativeto the earth’s coordinate frame.

When aligning the object so that the roll, pitch and yaw angles represent rotationabout the x, y and z axis respectively as seen in figure 2.2, the rotation matriceswill be as follows[9]:

Rx(φ) =

1 0 00 cos(φ) sin(φ)0 −sin(φ) cos(φ)

(2.3)

Ry(θ) =

cos(θ) 0 −sin(θ)0 1 0

sin(θ) 0 cos(θ)

(2.4)

Rz(ψ) =

cos(ψ) sin(ψ) 0−sin(ψ) cos(ψ) 0

0 0 1

(2.5)

These can be used to transform a vector under rotation. This is useful whentransforming the earth’s gravitational field vector to determine the position of an

7

CHAPTER 2. THEORY

object. By aligning the z-axis with the earth’s gravitational field (+g for z), definingthe roll, pitch and yaw as in figure 2.2 and also assuming no linear acceleration (fromvibrations etc.) the transformation becomes[9]:

Rxyz

001

= Rx(φ)Ry(θ)Rz(ψ)

001

=

−sin(θ)cos(θ)sin(φ)cos(θ)cos(φ)

(2.6)

Equation 2.6 can be rewritten in the form of equation 2.7 by relating the rolland pitch angles to the normalised accelerometer values Gp:

Gp

| Gp |=

−sin(θ)cos(θ)sin(φ)cos(θ)cos(φ)

→ 1√Gp

2x +Gp

2y +Gp

2z

Gpx

Gpy

Gpz

=

−sin(θ)cos(θ)sin(φ)cos(θ)cos(φ)

(2.7)

This makes it possible to calculate the roll and pitch angles using:

tan(φ) = Gpy

Gpz(2.8)

tan(θ) = −Gpx

Gpysin(φ) +Gpzcos(φ) = −Gpx√Gp

2y +Gp

2z

(2.9)

Equation 2.6 does not include any terms containing the yaw angle, this is thereason why it is not possible to calculate the yaw, pitch and roll by only usingaccelerometers.

2.4 PID controllerA proportional-integral-derivative controller (PID) is a feedback mechanism imple-mented to control a system. The PID controller is one of the most common typesof controller used in industries applications. Investigations and surveys from differ-ent industries show that more than 90 percent of all controllers in these industriesare implemented with PID or PI (no derivative term) regulation[10][11]. The PID-controller is based upon using the steady state error:

e(t) = r(t)− y(t) (2.10)

where r is the desired state and y is the current state, to calculate the neededinput (e.g. force from an engine) in order to reach a steady state. The input, u(t),is calculated through:

u(t) = Kpe(t) +Ki

∫ t

0e(t)dt+KD

d

dte(t) (2.11)

where Kp, Ki and Kd are all non-negative and represent the proportional, inte-gral and derivative terms respectively[12]. Figure 2.3 shows a block diagram overhow the PID-controller is implemented.

8

2.4. PID CONTROLLER

Figure 2.3. Block diagram showing the implementation of a PID-controller in asystem[13].

2.4.1 Servomotors and PWM

A servomotor uses pulse width modulation (PWM) to determine the angular posi-tion of the motors output shaft and they are typically used in application where highprecision is important. The servomotor shaft typically has an operating range thatspans between 0 and 180 degrees which makes it a suitable motor for controllingthe rods connected to the platform. Speed will also be an important factor to keepthe platform close to horizontal at all times. Servos follow a typical torque-speed-curve, i.e. high speed produces low torque and vice versa. The motor selection andcontrolling has to be done with consideration to ensure that the motor has the cor-rect operating speed while still producing a sufficient torque. The servo is thereforeregulated through pulse width modulation (PWM).

The signal to the servo can only be high (5 V) or low (0 V) at any time, butby controlling the time it is either high or low different signals can be sent. Thisis possible because the voltage regulation reacts quicker than the servomotor canregister. The concept duty cycle is frequently used to describe this. Duty cyclestells how much the signal is high in proportion to low in percentage. For examplea 75 percent duty cycle indicates that the digital signal is high 75 percent of thetime over an interval. The outcome of this is square waves, where 50 percent dutycycles become a perfect square wave. This is illustrated in figure 2.4.

9

CHAPTER 2. THEORY

Figure 2.4. Different duty cycles [14]

2.5 Microcontroller

A microcontroller is built up by a processor (CPU), memory and I/O peripherals.It can be considered as a small computer and it is an essential part in many hobbyprojects in the field of mechatronics. Raspberry Pi and Arduino are known manu-facturers of microcontrollers. Microcontrollers have two types of memory: programmemory and data memory. The program memory handles the instructions thatthe CPU processes. The data memory is used for temporary data storage while theprogram is running. The I/O peripherals are input and output devices that give themicrocontroller data from the sensors and produce electrical outputs which controlthe various devices that are connected to the board.

2.6 Mathematical Model

If the upper platform, the serving tray, of the model was locked horizontally, itsmotion when the rod from the servomotor pushes the platform upwards could bedescribed by the following mathematical model:

Let l be the length of the arm from the motor and L be the length of the con-nection (rod) from the arm to the upper platform. Also let α be the angle betweenthe connection and the horizontal plane and β the angle between the arm and thevertical plane, as shown in figure 2.5 where the rod is connected to the tray. Infigure 2.5, the vertical distance between the arm and the upper platform is:

Lsin(α) (2.12)

10

2.6. MATHEMATICAL MODEL

When β is zero degrees the distance is:

L+ l (2.13)

The servo motor can be modified to adjust β a certain angle. For each angle thatβ decreases, α increases by:

(90− arccos(l/L))/90 (2.14)

If β decreases with x degrees then the vertical distance increases by:

Lsin(α+ x(1− arccos(l/L))/90)− Lsin(α) + lsin(x) (2.15)

Figure 2.5. Servomotor connected to tray made in Paint

A pole is to be used in between the serving tray and the base platform to relieveload of the servo motors. Motion will be allowed by placing it in the middle of theplatform and attaching it to the tray using a ball joint. This mathematical model istherefore not sufficient since the serving tray will be locked vertically in the middleof the tray.

When the pole is attached to the middle of the serving tray the platform willfollow the motion displayed in figure 2.6 when the arm of the servomotor rotatesupward and therefore presses the tray upwards. This will mainly result in a verticalmotion for the connection between the arm and the tray, but also some horizontalmovement. The crosses in the figure represents the change in position for the rodof the servo.

The difference in vertical position:

Dsin(ϕ) (2.16)

Horizontal:D(1− cos(ϕ)) (2.17)

11

CHAPTER 2. THEORY

Figure 2.6. Tilting tray made in Paint

For small values for ϕ, the difference in sinus is much greater than the differencefor cosine. Knowing this and that the IMU and the servomotors are not very precise,it will be assumed that the connection is fixed horizontally and therefore followsthe relation stated in (2.15).

12

Chapter 3

Materials and Design

3.1 Design

The platform is designed in two layers consisting of a bottom-layer and a top layer.The bottom-layer platform is used to house the motors and all the electronics andwill also serve as a hand-held grip for the construction. The top-layer platform willrest on top of a ball-joint that connects the two platform. The ball-joint will allowthe platform to rotate freely and will serve as a necessary support for the armsthat goes from the motors to the platform which allows the motors to operate morerapidly.

Figure 3.1. Design of the platform made in Solid Edge ST9[15]

3.2 The TowerPro SG-5010 Servomotor

The servomotors used in this project are of type TowerPro SG-5010. It is a high-torque standard servo that can rotate approximately 180 degrees, meaning 90 de-

13

CHAPTER 3. MATERIALS AND DESIGN

grees in each direction. It should be powered with 4,8-6V DC max, and therefore5V is useable. It measures 40.8 x 20.1 x 38 mm (length-width-height) and weighs40 g[16]. The servos are shown in figure 3.2. The torque rate is 5 kg. cm at 4.8V and 6 kg. cm at 6V. It has three wires connected to the Arduino board whichconnect to power, ground and PWM control.

Figure 3.2. Servomotors used in the prototype[17]

3.3 The Inertial Measurement Unit LSM6DS3 bySparkFun

The measurement unit used in this project has six degrees of freedom and is oftype LSM6DS3, shown in figure 3.3. The board is a combination of accelerometers,

Figure 3.3. Inertial measurement unit used in the prototype[18]

gyroscopes and other detection utilities. Six degrees of freedom indicates that the

14

3.4. ELECTRICAL CIRCUIT

IMU includes three accelerometers and three gyroscopes. The IMU also has a 8 kBfirst in first out buffer (FIFO) which deloads the processor through fewer detectionsfrom the sensor[19]. It can be used to detect motion, tilt, shocks and temperature.It allows I2C and SPI communication. I2C is used in this project.

3.4 Electrical CircuitFigure 3.4 shows the electrical circuit for the prototype and table 3.1 shows thecomponents of the circuit. The Arduino board and the servo motors are poweredthrough an external power source. The power source converts 230 V to 5 V andsupplies 3000 mA.

Figure 3.4. Circuit diagram made in Fritzing[20]

15

CHAPTER 3. MATERIALS AND DESIGN

——————— ——————————————-# Components1 Arduino Uno Rev31 IMU -LSM6DS32 Servo - TOWERPRO SG-50101 5V External Power Supply

Table 3.1 Components of the circuit.

3.5 Controller for the Platform

The control design in this project will aim to make the system responsive yetsmooth. One key-part in designing the controller for the serving tray is to an-alyze the mechanical behavior of a system and how the error term is setup.

Here, the error term will be represented by the angle offset of the serving tray.As a result, the error will go to zero as the tray approaches a horizontal position.In order to achieve a fast response time, Kp has been set-up to give a significantmotor gain in situations where the angle offset is large.

However, too large values for Kp tend to result in large instabilities around thereference point. The goal of tuning Kp has thus been to make it as quick as pos-sible without causing oscillations. This in turn cause a large steady state error asthe proportional-controller won’t be able to fully balance the tray. This has beenresolved with the integral controller, which drives down remaining steady-state er-ror and helps the controller to keep the tray horizontal at small error angles.

Both Kp and Ki produce motor gain and it has been crucial for the system respon-siveness to keep these constants as large as possible. Some of the oscillations thatarise from the constants could be resolved through tuning the Kd, which counter-actrapid error-changes.

3.6 The Code for the Platform

The first part of the code consists of declaring all the necessary variables. Theheader files for the IMU and the servos are then included. Error terms for thecalibration of the IMU has been set through tests. After declaring the variables thesetup section is run. Here, the IMU is woken up and the servos are attached totheir respective pins. In the loop section of the code the roll and pitch values arecalculated through the readings of the accelerometers and gyroscopes. The filtersare then applied to the values for a better estimation of the angles. Lastly the

16

3.6. THE CODE FOR THE PLATFORM

PID-controller is applied and the desired position of the servomotors are received.If the position is in the accepted interval the servos will turn to this position.

Figure 3.5. Flowchart over how the code for the platform works, made in draw.io[21]

17

Chapter 4

Results

The results presented here aim to showcase the outcome of how the platform reactedwhen a step-response was introduced to the system, and how the different values onKp, Ki and Kd build up the PID response. The tests were executed as described inchapter 1.3 and the results here show the performance from one of the motors.

4.1 PID response without weight distributionThe results plotted in figure 4.1, 4.2 and 4.3 shows the step-response from theplatform when no weight bias was present.

19

CHAPTER 4. RESULTS

Figure 4.1. Kp = 4, Ki = 0, Kd = 0

20

4.1. PID RESPONSE WITHOUT WEIGHT DISTRIBUTION

Figure 4.2. Kp = 1.35, Ki = 0.57, Kd = 0

21

CHAPTER 4. RESULTS

Figure 4.3. Kp = 1.35, Ki = 0.57, Kd = 0.08

22

4.2. PID RESPONSE WITH WEIGHT DISTRIBUTION

4.2 PID response with weight distributionThe following result shows the step-response when a weight bias of 250g was addedto the platform. Figure 4.4 shows the old PID tuning and figure 4.5 shows the PIDwhen a larger gain had been added.

Figure 4.4. 250 gram weight Kp = 1.35, Ki = 0.57, Kd = 0.08

23

CHAPTER 4. RESULTS

Figure 4.5. 250 gram weight Kp = 1.5, Ki = 0.57, Kd = 0.04

24

Chapter 5

Discussion

The results show that the proportional controller in our case produced a largesteady-state error and that further increase of Kp would result in a lot of instabil-ity. Here, Ki was important to generate a swift response-time while at the sametime be responsible for driving down the steady-state error. However, the fastresponse-time also created much unwanted instability around the reference point.

The derivation part of the controller helped out to bring down some of the in-stability, but was generally quite hard to get in tune through out the project. Onereason for this could be that the sensoring data from the IMU produced too muchnoise for the derivation part to cope with.

When a weight bias was added to the platform it became evident that a largergain was needed to drive down the error term. However increasing Ki at this pointwould most likely create large instability at the reference point. Our conclusion wasto instead increase the P-controller as most of the gain were needed at large off-setangles.

25

Chapter 6

Conclusion

Research and tests made in the project makes it possible to answer the research ques-tions.

How do we use the information from the inertial measurement unit (IMU) to sta-bilize the platform?

This could be done through determining rotation through readings from the gy-roscopes and accelerometers. These could be combined with a complementary filterto achieve a more accurate estimation of the rotation angles. The angles are thenused to determine how much the servomotors have to rotate in order to keep theplatform horizontal to the ground.

How will variations in PID constants affect the performance in regards to speedregulations and smoothness?

The proportionate part were the most important factor for speed. A high Kp how-ever caused a lot of gain which in some cases led to oscillations. The integral partwas essential in order to remove the steady-state error. The tuning of the derivativepart was not very successful. The reason for this has been researched thoroughlybut with little success.

How will differences in load and weight distribution affect the PID-controller?

The controller needed to provide more gain in order to produce a sufficient step-response when a weight was added. In our case this gain was best achieved byincreasing Kp rather than Ki since most of the gain was needed at large error. Theadded weight also produced more, jiggly, instability which was exaggerated by thederivation control. The need of tuning down Kd was therefore necessary to avoidlarge spikes in the derivation controller.

27

CHAPTER 6. CONCLUSION

The serving tray will unfortunately not be useful to serve beverages. It is cur-rently too unstable because one of the servomotors is unreliable. If both of theservos performed as good as the one which the tests were executed with, it mighthave been a useful product. The electronics would obviously have to be covered assome spillage might happen.

28

Chapter 7

Recommendations for Future Work

The platform could be improved in several ways. The most important improvementwould most likely be more reliable servomotors. One of the servomotors had signif-icant worse performance than the other. The servomotors should however probablyhave been placed so that a change in the angle for the motor arm caused a lesserchange in the angle of the platform. This could perhaps cause less oscillation andtherefore allow quicker regulation. A handle like the one in figure 3.1 could be madeto assist the motors by making it easier to hold stable. Another recommendationwould be using the inertial measurement unit MPU6050 instead of the LSM6DS3as the available tutorials and code examples for this IMU are far superior becauseit is widely used. It is also cheaper and possibly more accurate. Using an IMUwith magnetometers could be useful as well as it would solve the problem with thegyroscope drifting.The controller could be improved through more extensive tests or by using a math-ematical model to calculate the transfer function. If the PID constants would begenerated through tests it would be recommended to use MATLAB’s function PID-tuning and Simulink. The transfer function of a system can be estimated throughMATLAB’s function System Identification which uses samples of the inputs andoutputs of a system. The derivative part of the controller mostly caused issues forthe stabilization. More focus should have been put in identifying the reason for thisand optimal values for Kd. The complementary filter worked well, although minorunwanted fluctuations in the angles occur. An alernative could be to implement themore advanced Kalman filter.

29

Bibliography

[1] Stewart. D. “A platform with six degrees of freedom”. In: Proceedings of theInstitution of Mechanical Engineers 180 (), pp. 371–386. [Online] Retrievedfrom: https://journals.sagepub.com/doi/abs/10.1243/PIME_PROC_1965_180_029_02 [Accessed: 20.03.2020].

[2] Acrome Robotics. Stewartplatform image. [Online] Retrieved from https://acrome-net.s3.us-east-2.amazonaws.com/uploads\protect\penalty-\@M/47099a09c89a4eabac6660af839378cc.jpg [Accessed: 02.05.2020].

[3] Johan Abrahamsson and Johan Danmo. The Stabilizing Spoon: Self-stabilizingutensil to help people withimpaired motor skills. 2017.

[4] Adobe illustrator. [Software] Available at: https://www.adobe.com/se/products/illustrator.html.

[5] MATLAB. [Software] Available at: https://www.mathworks.com/products/get-matlab.html.

[6] H. Wymeersch G. Seco-Granados J. Nurmi E. Lohan and O. Nykanen. Multi-Technology Positioning. 2017, pp. 87–88. isbn: 978-3-319-50427-8. [Online]Retrieved from: https://bit.ly/2Wu4DWn, [Accessed: 05.03.2020].

[7] Walter T Higgins. “A comparison of complementary and Kalman filtering”. In:IEEE Transactions on Aerospace and Electronic Systems 3 (1975), pp. 321–325. [Online] Retrieved from: https://ieeexplore-ieee-org.focus.lib.kth.se/document/4101411 , [Accessed: 05.07.2020].

[8] Howard Musoff and Jerold P Gilmore. Inertial navigation system with auto-matic redundancy and dynamic compensation of gyroscope drift error. Mar.1993. US Patent 5,194,872.

[9] Mark Pedley. Tilt Sensing Using a Three-Axis Accelerometer. 2013. [Online]Retrieved from: https://www.nxp.com/files-static/sensors/doc/app_note/AN3461.pdf [Accessed: 04.02.2020].

[10] Karl Johan Astrom and Tore Hagglund. “Revisiting the Ziegler–Nichols stepresponse method for PID control”. In: Journal of process control 14.6 (2004),pp. 635–650. [Online] Retrieved from: https://www.sciencedirect.com/science/article/pii/S09591524040000341 , [Accessed: 04.29.2020].

31

BIBLIOGRAPHY

[11] Karl Johan Astrom, Tore Hagglund, and Karl J Astrom. Advanced PID con-trol. Vol. 461. ISA-The Instrumentation, Systems, and Automation SocietyResearch Triangle . . ., 2006. [Online] Retrieved from: http : / / intranet .ceautomatica.es/sites/default/files/\protect\penalty-\@Mupload/13/files/AdvancesInPIDControl_KJA.pdf , [Accessed: 04.29.2020].

[12] T. Glad and L. Ljung. Reglerteknik: Grundlaggande teori. 4th ed. Studentlit-teratur, 2006. isbn: 9789144022758.

[13] Control Engineering. PID image. [Online] Retrieved from https : / / www .controleng.com/wp-content/uploads/sites/2/2014/08/CTL1408_WEB_F1_PID-Valin_Fig1_PID-control_loopslider.jpg [Accessed: 02.05.2020].

[14] Sparkfun. Pulse Width Modulation. [Online] Retrieved from https://cdn.sparkfun.com/assets/f/9/c/8/a/512e869bce395fbc64000002.JPG [Ac-cessed: 19.3.2020].

[15] Solid Edge ST9. [Software] Available at: https://solidedge.siemens.com/en/.

[16] Feetech. Towerpro SG-5010 datasheet. [Online] Retrieved from https://www.electrokit.com/uploads/productfile/41003/FS5106B_specs.pdf [Ac-cessed: 29.3.2020].

[17] OPOSYS. Towerpro SG-5010 image. [Online] Retrieved from https://cdn.shopify.com/s/files/1/2641/6180/products/product-image-429283132_1024x1024@2x.jpg?v=1575431147 [Accessed: 29.3.2020].

[18] Electrokit. LSM6DS3. [Online] Retrieved from https://www.electrokit.com/uploads/productimage/41013/41013994.jpg [Accessed: 03.17.2020].

[19] Sparkfun. Datasheet LSM6DS3. [Online] Retrieved from https://cdn.sparkfun.com/assets/learn_tutorials/4/1/6/DM00133076.pdf [Accessed: 03.17.2020].

[20] Fritzing. [Software] Available at: https://www.electroschematics.com/fritzing-software-download/.

[21] Draw.io. [Software] Available at: https://www.draw.io/.

32

Appendix A

Arduino Code

// Author : Marcus Akki la and Bix Er iksson// Name o f the p r o j e c t : S e l f−S t a b i l i z i n g Platform// TRITA NUMBER: ITM−EX 2020:49// Date : 2020−05−13

// IMU connect i ons :// Vcc to 3 .3 V// GND to GND// SCL to A5// SDA to A4

// Servos connected to pin 9 and 10

// Inc lud ing nece s sa ry l i b r a r i e s .

#inc lude ”SparkFunLSM6DS3 . h”#inc lude ”Wire . h”#inc lude <Servo . h>

// Dec lar ing hardwareLSM6DS3 myIMU; // Defau l t con s t ruc to r i s I2C , addr 0x6BServo myservoX ;Servo myservoY ;

// Dec lar ing v a r i a b l e sf l o a t accX , accY , accZ ;f l o a t Rol lacc , Pitchacc , Rol l , Pitch ;

f l o a t GyroX , GyroY ;

33

APPENDIX A. ARDUINO CODE

f l o a t gyroAngleX , gyroAngleY ;f l o a t Rol lgyro , Pitchgyro ;f l o a t elapsedTime , currentTime , previousTime , testTime = 0 ;

f l o a t Ro l l Se rvo ;f l o a t Pi tch Servo ;

//PID constant sf l o a t Kp x = −1.35; //1 2 .5 //0 .6f l o a t Kp y = 1 . 1 ;

f l o a t Kd x = −0.08; //0 .03 4 // 0 .06f l o a t Kd y = 0 ;

f l o a t Ki x = −0.57; //0 .07f l o a t Ki y = 0 . 1 5 ;

// Var iab l e s f o r D−part o f the c o n t r o l l e rf l o a t ErrorDi f f e renceX ;f l o a t rateErrorX ;f l o a t las tErrorX = 0 ;

f l o a t ErrorDi f f e renceY ;f l o a t rateErrorY ;f l o a t las tErrorY = 0 ;

f l o a t PID px ;f l o a t PID py ;f l o a t PID dx ;f l o a t PID dy ;f l o a t PID ix ;f l o a t PID iy ;f l o a t PID x ;f l o a t PID y ;

void setup ( ) {// S e r i a l . begin (115200 ) ; // Baudrate 115200 i f s e r i a l p r i n t s// are sent to MATLAB.S e r i a l . begin ( 9 6 0 0 ) ; // Baudrate 9600 to use// s e r i a l monitor f o r s e r i a l p r i n t s .de lay ( 1 0 0 0 ) ; // r e l a x . . .myIMU. begin ( ) ; //Wake up IMU

// Attach s e rvo s to a s s o c i a t e d p ins

34

myservoX . attach ( 1 0 ) ;myservoY . attach ( 9 ) ;myservoX . wr i t e ( 9 0 ) ;myservoY . wr i t e ( 9 0 ) ;de lay ( 5 0 0 0 ) ;

}

void loop ( ) {

//−−Accelerometer−−//accX = myIMU. readFloatAccelX ( ) ;accY = myIMU. readFloatAccelY ( ) ;accZ = myIMU. readFloatAcce lZ ( ) ;

//Remove e r r o r by c a l i b r a t i n g the ac c e l e r omet e r s//( va lue s are c a l c u l a t e d through t e s t i n g s )accX = accX + 0 . 0 2 ;accY = accY − 0 . 0 4 ;accZ = accZ + 0 . 0 2 ;

// Ca l cu la te Rol l and Pitch//( r o t a t i o n around X−axis , r o t a t i o n around Y−a x i s )Rol l = atan ( accY/accZ ) ∗ 180 / PI ;Pitch = atan ( accX/accZ ) ∗ 180 / PI ; // Pitch can be// est imated in both o f the se ways .// Pitch = atan(−accX / s q r t (pow( accY , 2) + pow( accZ , 2 ) ) ) ∗ 180 / PI ;

Ro l l acc = Rol l ;P i tchacc = Pitch ;

//−−Gyroskop−−//// Previous time i s s to r ed be f o r e the ac tua l time readpreviousTime = currentTime ;currentTime = m i l l i s ( ) ; // Current time ac tua l time readelapsedTime = ( currentTime − previousTime )/1000 ; //Time in mstestTime = testTime + elapsedTime ;GyroX = myIMU. readRawGyroX ( ) / 1 4 . 4 ; // Divide by s e n s i t i v i t yGyroY = myIMU. readRawGyroY ( ) / 1 4 . 4 ;

// Correct the outputs with the c a l c u l a t e d e r r o r va lue sGyroX = GyroX + 5 ; // GyroErrorX ˜(+5)GyroY = GyroY + 2 . 5 ; // GyroErrorY ˜(+2.5)

35

APPENDIX A. ARDUINO CODE

// Current ly the raw va lues are in degree s per seconds , deg/s ,// t h e r e f o r e mult ip ly by seconds ( s ) to get the ang le in degree s .Ro l lgyro = Rol l Se rvo + GyroX ∗ elapsedTime ; // deg/ s ∗ s = degPitchgyro = Pitch Servo + GyroY ∗ elapsedTime ;

//Complementary f i l t e rRo l l Se rvo = 0 .8 ∗ Rol lgyro + 0 .2 ∗ Rol lacc ;P i tch Servo = 0 .8 ∗ Pitchgyro + 0 .2 ∗ Pitchacc ;

i f ( testTime > 8){ //Wait u n t i l testTime i s 8 s to// get c o r r e c t p l o t s in MATLAB

// Ca lcu la te the d e r i v a t i v e o f the e r r o rErrorDi f f e renceX = Rol l Se rvo − l a s tErrorX ;rateErrorX = ErrorDi f f e renceX / elapsedTime ;las tErrorX = Rol l Se rvo ;

ErrorDi f f e renceY = Pitch Servo − l a s tErrorY ;rateErrorY = ErrorDi f f e renceY / elapsedTime ;las tErrorY = Pitch Servo ;

// Ca l cu la te the PID output

PID px = Kp x∗ Rol l Se rvo ;PID py = Kp y∗ Pitch Servo ;

PID dx = Kd x∗ rateErrorX ;PID dy = Kd y∗ rateErrorY ;

// Ca l cu la te the I outputPID ix = PID ix + ( Ki x ∗ Rol l Se rvo ) ;PID iy = PID iy + ( Ki y ∗ Pitch Servo ) ;

//Combine the va lue s in to the PID outputPID x = PID px + PID dx + PID ix ;PID y = PID py + PID dy + PID iy ;

// Boundaries f o r the servoi f ( PID ix > 55) {PID ix = 55 ;}i f ( PID iy > 15) {PID iy = 15 ;}

36

i f ( PID ix < −55) {PID ix = −55;}i f ( PID iy < −15) {PID iy = −15;}

i f ( PID x > 65) {PID x = 65 ;}i f ( PID y > 15) {PID y = 15 ;}

i f ( PID x < −60) {PID x = −60;}i f ( PID y < −15) {PID y = −15;}

// Pr in t s ad justed f o r the MATLAB program// S e r i a l . p r i n t ( Ro l l ) ;// S e r i a l . p r i n t (” ” ) ;// S e r i a l . p r i n t ( PID x ) ;// S e r i a l . p r i n t (” ” ) ;// S e r i a l . p r i n t ( PID px ) ;// S e r i a l . p r i n t (” ” ) ;// S e r i a l . p r i n t ( PID dx ) ;// S e r i a l . p r i n t (” ” ) ;// S e r i a l . p r i n t l n ( PID ix ) ;

// S e r i a l . p r i n t l n ( Pi tch Servo ) ;// S e r i a l . p r i n t l n ( PID y ) ;// S e r i a l . p r i n t l n ( PID py ) ;// S e r i a l . p r i n t l n ( PID dy ) ;// S e r i a l . p r i n t l n ( PID iy ) ;// S e r i a l . p r i n t l n ( testTime −8);

myservoX . wr i t e ( PID x + 9 0 ) ;

S e r i a l . p r i n t ( Pi tch Servo ) ;S e r i a l . p r i n t (” ” ) ;S e r i a l . p r i n t (90 + PID y ) ;S e r i a l . p r i n t (” ” ) ;// Current p o s i t i o n o f the servoS e r i a l . p r i n t l n ( myservoY . read ( ) ) ;

myservoY . wr i t e ( PID y + 9 0 ) ;

de lay ( 2 0 ) ;}}

37

Appendix B

MATLAB Code

1 c l e a r a l l , c l o s e a l l , c l c2

3 %Need Arduino add−on4

5 %Baudrate on 115200 both in MATLAb and Arduino .6 a = s e r i a l ( ’COM3’ , ’ Baudrate ’ ,115200) ; %S e r i a l read f o r port

COM3.7 fopen ( a ) ;8 r e s u l t = [ ] ;9 f o r x=1:6000 %Read 1000 loops

10 output = f s c a n f ( a , ”%f ”) ; %Get s e r i a l p r i n t s in the formo f f l o a t .

11 r e s u l t =[ r e s u l t , output ] ; %Store va lue s .12 end13

14 %Declare vec to r to s t o r e va lue s in .15 r o l l = [ ] ;16 p i t ch = [ ] ;17 PID x = [ ] ;18 PID dx = [ ] ;19 PID ix = [ ] ;20 PID px = [ ] ;21 time = [ ] ;22

23 %Close the connect ion to the Arduino .24 f c l o s e ( a ) ;25 d e l e t e ( a ) ;26

27 d = 1 ;28 %Store va lue s in a s s o c i a t e d v ec to r s .

39

APPENDIX B. MATLAB CODE

29 whi le d < l ength ( r e s u l t )30 r o l l = [ r o l l r e s u l t (d) ] ;31 d = d+1;32 PID x = [ PID x r e s u l t (d) ] ;33 d = d+1;34 PID px = [ PID px r e s u l t (d) ] ;35 d = d+1;36 PID dx = [ PID dx r e s u l t (d) ] ;37 d = d+1;38 PID ix = [ PID ix r e s u l t (d) ] ;39 d = d+1;40 time = [ time r e s u l t (d) ] ;41 d = d+1;42 end43

44 %Plot va lue s in subplot .45 f i g u r e46 subplot ( 2 , 1 , 1 )47 p lo t ( time , PID x )48 r e f = 3 3 . 5 ;49 hold on50 p lo t ( time , ones ( s i z e ( time ) ) ∗ r e f )51 hold on52 l egend ( ’PID ’ , ’ Re ference ’ )53 y l a b e l ( ’ Angle ’ )54 x l a b e l ( ’Time ’ )55 ylim ( [ −50 ,40 ] )56 xlim ( [ 0 , time ( end ) ] )57

58

59 % ylim ( [ −50 ,40 ] )60 % xlim ( [ 0 , time ( end ) ] )61 % f i g u r e62 % subplot ( 2 , 1 , 1 )63 % plo t ( time , PID x )64 % hold on65 % plo t ( time , r o l l )66 % legend ( ’PID ’ , ’ Rol l ’ )67 % y l a b e l ( ’ Angle ’ )68 % x l a b e l ( ’ Time ’ )69

70 subplot ( 2 , 1 , 2 )71 p lo t ( time , PID px )72 hold on

40

73 p lo t ( time , PID ix )74 hold on75 p lo t ( time , PID dx )76 l egend ( ’P ’ , ’ I ’ , ’D ’ ) ;77 ylim ([−45 4 5 ] ) ;78 x l a b e l ( ’ time ’ )

41

TRITA ITM-EX 2020:49

www.kth.se