Self-Stabilizing Platform

Thesis WIP

MARCUS AKKILABIX ERIKSSON

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

TRITA ITM 2020:49

Abstract

This project explores the possibility to stabilize a handheld serving-tray using a micro-controller, two servomotorsand an inertial measurement unit (IMU). It is heavily fo-cused on control theory, specifically using a PID controller.The report covers the theory necessary to construct a selfstabilizing platform and describes involving components inthe prototype and how they cooperate. With the gyro-scopes and accelerometers involved in the IMU it is possi-ble to determine orientation and rotation. The constructionenables rotation about the x-axis (roll) and y-axis (pitch)but not the z-axis (yaw). The readings from the gyroscopesand the accelerometers are combined and filtered througha complementary filter to estimate the rotations. The ser-vomotors compensate disturbance in keeping the platformhorizontal through PID regulation. The PID constants aretuned through tilting the platform a specific angle and plot-ting the step response 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. Rapporten tacker relevant te-ori for att konstruera en sjalvstabiliserande platform ochbeskriver ingaende komponenter i prototypen samt hur desamverkar. Med gyroskopen och accelerometrarna som finnsi IMU:n ar det mojligt att uppskatta position och rota-tion for ett objekt. Matningarna fran gyroskopen och acce-lerometrarna kombineras och filtreras med hjalp av ett s.k.complementary filter for att uppskatta rotationen av objek-tet. Servomotorerna anvands i sin tur till att halla plattanhorisontell genom att kompensera storningar fran omgiv-ningen. Detta gors genom PID-reglering. Konstanterna iPID-regulatorn ar framtagna genom tester dar plattformenlutas ett bestamt antal grader och stegsvaret plottas i MAT-LAB.

Nyckelord: Mekatronik, PID-regulator, Reglerteknik, Ar-duino, Servomotor, IMU, 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 Tage Rahlen and Anton Hedstrom for their good feedback onthe report.

Contents

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

2 Theory 32.1 Inertial Measurement Unit (IMU) . . . . . . . . . . . . . . . . . . . . 32.2 Filtering Disturbances . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2.1 Complemantary Filter . . . . . . . . . . . . . . . . . . . . . . 32.3 Calculating Roll and Pitch Angles Using Accelerometers . . . . . . . 42.4 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.4.1 Servomotors and PWM . . . . . . . . . . . . . . . . . . . . . 82.5 Microcontroller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.6 Mathematical Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3 Materials and Design 113.1 Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2 The TowerPro SG-5010 Servomotor . . . . . . . . . . . . . . . . . . . 113.3 The Inertial Measurement Unit LSM6DS3 by SparkFun . . . . . . . 123.4 Electrical Circuit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.5 Controller for the Platform . . . . . . . . . . . . . . . . . . . . . . . 143.6 The Code for the Platform . . . . . . . . . . . . . . . . . . . . . . . 14

4 Results 174.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Result of step-responses . . . . . . . . . . . . . . . . . . . . . . . . . 18

5 Discussion 23

6 Conclusion 25

7 Recommendations for Future Work 27

Bibliography 29

Appendices 30

A Arduino Code 31

Appendices 36

B MATLAB Code 37

List of Figures

1.1 A Stewart Platform [17] . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

2.1 Complementary filter made in paint . . . . . . . . . . . . . . . . . . . . 42.2 Orientation of the serving tray . . . . . . . . . . . . . . . . . . . . . . . 52.3 Block diagram showing the implementation of a PID-controller in a sys-

tem[7]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.4 Different duty cycles [20] . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.5 Servo connected to tray made in Paint . . . . . . . . . . . . . . . . . . . 102.6 Tilting tray made in Paint . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.1 Design of the platform made in Solid Edge ST9[18] . . . . . . . . . . . . 113.2 Servomotors used in the prototype[15] . . . . . . . . . . . . . . . . . . . 123.3 Inertial measurement unit used in the prototype[6] . . . . . . . . . . . . 123.4 Circuit diagram made in Fritzing[9] . . . . . . . . . . . . . . . . . . . . . 133.5 Flowchart over how the code for the platform works, made in draw.io[5] 15

4.1 Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Kp=4, Ki=0, Kd = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.3 Kp=1.35, Ki=0.57, Kd = 0 . . . . . . . . . . . . . . . . . . . . . . . . . 194.4 Kp=1.35, Ki=0.57, Kd=0.08 . . . . . . . . . . . . . . . . . . . . . . . . 204.5 250 gram weight Kp=1.35, Ki=0.57, Kd=0.08 . . . . . . . . . . . . . . . 214.6 250 gram weight Kp=1.5, Ki=0.57, Kd=0.04 . . . . . . . . . . . . . . . 22

List of Tables

3.1 Table over components . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

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 servo motors to be able to balanceobjects on it. It can detect position and motion of the object using sensors andthen compensate the motion using its six motors allowing adjustments in six de-grees of freedom. Figure 1.1 shows a Stewart platform made by Acrome Robotics.With good equipment it can react very fast and with great precision. The firstStewart Platform was presented in 1965 in form of a flight simulator[4]. It is nowwidely used in industries such as aerospace, nautical and automotive technology.

Figure 1.1. A Stewart Platform [17]

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[1].

1.2 PurposeThe 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 MethodThe 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 you walk with it. The servomotors are placed on topof a base platform perpendicular to each other. These are used to control the angleof a platform (the serving tray) on top of the base platform. A rod with a ball jointis used in the middle of the platform to support the servomotors.

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.

2

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[12]. 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[11]. When it is applied on the values received from the IMU it evalu-ates an estimated angular position by mostly using the information received fromthe gyroscope, 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,

3

CHAPTER 2. THEORY

meaning that it introduces error in the output as time passes[14]. The complemen-tary filter 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 mostlylow frequency. It should therefore be filtered with a low pass filter G(s)[11]. Theopposite applies 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)

4

2.3. CALCULATING ROLL AND PITCH ANGLES USING ACCELEROMETERS

Figure 2.2. Orientation of the serving tray

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[16]:

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)

5

CHAPTER 2. THEORY

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 anobject. 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[16]:

Rxyz

001

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

=(

cos(θ)cos(ψ) cos(θ)sin(ψ) −sin(θ)cos(ψ)sin(θ)sin(φ) − cos(θ)sin(ψ) cos(φ)cos(ψ) + sin(θ)sin(φ)sin(ψ) cos(θ)sin(φ)cos(φ)cos(ψ)sin(θ) + sin(φ)sin(ψ) cos(φ)sin(θ)sin(ψ − cos(ψ)sin(θ) cos(θ)cos(φ)

)(001

)(2.7)

=

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

(2.8)

Equation 2.8 can be rewritten in the form of equation 2.9 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.9)

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

tan(φ) = Gpy

Gpz(2.10)

tan(θ) = −Gpx

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

2y +Gp

2z

(2.11)

Equation 2.8 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 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 types ofcontroller used in industries applications. Investigations and surveys from differentindustries and show that more than 90 percent of all controllers in these indus-tries are implemented with PID or PI (no derivative term) regulation[2][3]. ThePID-controller is based upon using the steady state error:

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

6

2.4. CONTROLLER

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.13)

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

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

7

CHAPTER 2. THEORY

2.4.1 Servomotors and PWMA 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 controlling therods of the platform. Speed will also be an important factor to keep the platformclose to horizontal at all times. Servos follow a typical torque-speed-curve, i.e. highspeed produces low torque and vice versa. The motor selection and controlling hasto be done with consideration to ensure that the motor has the correct operatingspeed while still producing a sufficient torque. The servo is therefore regulatedthrough pulse width modulation (PWM).

The signal to the servo can only be high (5V) or low at any time, but by con-trolling the time it is either high or low different signals can be sent. The conceptduty cycle is frequently used to describe this. Duty cycles tells how much the signalis high in proportion to low in percentage. For example a 75 percent duty cycleindicates that the digital signal is high 75 percent of the time over an interval. Theoutcome of this is square waves, where 50 percent duty cycles become a perfectsquare wave. This is illustrated in figure 2.4.

Figure 2.4. Different duty cycles [20]

2.5 MicrocontrollerA 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-

8

2.6. MATHEMATICAL MODEL

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 ModelIf the upper platform, the serving tray, of the model was locked horizontally, itsmotion could be described by the following mathematical model: Let l be the lengthof the arm from the motor and L be the length of the connection from the arm to theupper platform. Also let α be the angle between the connection and the horizontalplane and β the angle between the arm and the vertical plane, as shown in figure2.5. In figure 2.5, the vertical distance between the arm and the upper platform is:

Lsin(α) (2.14)

When β is zero degrees the distance is:

L+ l (2.15)

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

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

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

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

A rod 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 the rod in the middleof the platform and attaching it to the tray using a ball joint. This mathematicalmodel is therefore not sufficient since the serving tray will be locked vertically inthe middle of the tray.

When the rod is attached to the middle of the serving tray it will follow themotion displayed in figure 2.6 when the arm of the servomotor rotates upward andtherefore presses the tray upwards. This will mainly result in a vertical motion forthe connection between the arm and the tray, but also some horizontal movement.The crosses in the figure represents the change in position for the connection.

The difference in vertical position:

Dsin(ϕ) (2.18)

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

9

CHAPTER 2. THEORY

Figure 2.5. Servo connected to tray made in Paint

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.17).

10

Chapter 3

Materials and Design

3.1 DesignThe 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[18]

3.2 The TowerPro SG-5010 ServomotorThe 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-

11

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 weighs 40g[8]. The servos are shown in figure 3.2. The torque rate is 5 kg. cm at 4.8 V and6 kg. cm at 6V. It has three wires connected to the arduino board which connectto power, ground and PWM control.

Figure 3.2. Servomotors used in the prototype[15]

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[6]

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

12

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 Circuit

Figure 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[9]

Table 3.1 Components of the circuit

13

CHAPTER 3. MATERIALS AND DESIGN

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

3.5 Controller for the PlatformThe control design in this project will aim to make the system responsive butyet smooth. One key-part in designing the controller for the serving tray is toanalyze 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.This position will serve as an equilibrium point as the the forces working on thesystem will cancel each other out, effectively making any motor gain undesired asthe tray are evenly balanced.

The integral part of the PID would in this situation create an un-beneficial inputwhen the signal is being fed back into the system, hence, the integral part are not be-ing used in this project. The control design have instead revolved around creating aPD-controller.

3.6 The Code for the PlatformThe 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 thePD-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.

14

3.6. THE CODE FOR THE PLATFORM

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

15

Chapter 4

Results

4.1 MethodThe performance of the PID has been analyzed by introducing a step-response to thesystem. The step-response in this case consists of tilting the tray from an horizontalposition, which in turn will create an angle offset error α that the PID will try tocompensate.

Figure 4.1. Experimental setup

17

CHAPTER 4. RESULTS

The PID response have been registered and plotted using MATLAB[13]. Theaim of the test is to see how the P, I and D parts of the controller work together tostabilize the platform, and how this relationship change when a weight distributionis added to the platform.

4.2 Result of step-responsesThe following result shows how the step-response of the system changed based onthe different PID tunings. The results plotted in figure 4.2, 4.3 and 4.4 shows thestep-response from the platform when no weight bias was present.

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

18

4.2. RESULT OF STEP-RESPONSES

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

19

CHAPTER 4. RESULTS

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

20

4.2. RESULT OF STEP-RESPONSES

The following result shows the step-response when a weight bias of 250g wasadded to the platform. Figure 4.5 shows the old PID tuning and figure 4.6 showsthe PID when a larger gain had been added.

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

21

CHAPTER 4. RESULTS

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

22

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.

23

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 with 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.

25

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.

26

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.

27

Bibliography

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

[2] 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].

[3] 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/upload/13/files/AdvancesInPIDControl_KJA.pdf , [Accessed: 04.29.2020].

[4] 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].

[5] Draw.io. [Software] Available at: https://www.draw.io/.[6] Electrokit. LSM6DS3. [Online] Retrieved from https://www.electrokit.

com/uploads/productimage/41013/41013994.jpg [Accessed: 03.17.2020].[7] 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].

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

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

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

29

BIBLIOGRAPHY

[11] 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].

[12] 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].

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

[14] 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.

[15] 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].

[16] 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].

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

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

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

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

30

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 ;

31

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

32

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)

33

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 PD 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 ;}

34

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 ) ;}}

35

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 .

37

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

38

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 ’ )

39