Tool orientation estimation to control the angle tightening process of threaded joints

(1)

IN

DEGREE PROJECT MECHANICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2019,

Tool orientation estimation to control the angle tightening process of threaded joints

MAX THIEL

(2)

(3)

Abstract

The most common method for securing components to each other during manufacturing of products is by joining these using screws, nuts and bolts. The benefit of using this method is that it is cheap and makes it easy to join and separate components quickly. The clamping force in the threaded joint is critical to the quality and in some respect the life length of the product, which makes it important to have good control of the clamping force. There are two main tightening strategies used when tightening a threaded joint – torque controlled tightening and angle controlled tightening. The first method monitors the applied torque during the entire tightening and halts when the target torque is reached. The second method, angle controlled tightening, measures the rotation of the threaded fastener in the joint. This method generally produces more accurate results with less scatter in the final clamping force. In order to apply angle controlled tightening using a hand-held tool it is required to not only control the output angle of the tool, but also how the tool moves in relation to the joint. This is to ensure that the control signal from the motor actually translates to clamping force in the joint and not to rotation of the tool itself.

This thesis project aims to analyze data from an IMU (Inertial Measurement Unit) built into a hand-held tightening tool in order to estimate tool movement and thereby react to undesired tool movement. An analysis has been performed to evaluate how the two sensor fusion methods – Kalman filter and Particle filter – perform in terms of estimating the orientation of the tool by combining measurements from the IMU’s accelerometers and gyroscopes.

Data was collected from the tool IMU during a number of angle tightening sequences with varying setups. Test were performed both for when the tool was kept still during the entire tightening and for when the tools was allowed to move freely. Tests were also carried out for a couple of different tool orientations to better understand the behavior of the two sensor fusion models.

The results from the tests showed that the Kalman Filter was able to better estimate the tool orientation. Especially in terms of accuracy, repeatability and reliability.

Keywords: angle controlled tightening, tool orientation, extended kalman filter, particle filter, quaternions

Master of Science Thesis TRITA-ITM-EX 2019:683

Tool orientation estimation to control the angle tightening process of threaded joints

Max Thiel

Approved Examiner

Hans Johansson

Supervisor

Bengt Eriksson

Commissioner

Atlas Copco Industrial Technique

Contact Person

Henrik Öhrlund

(4)

(5)

Examensarbete TRITA-ITM-EX 2019:683

Estimering av ett verktygs orientering för att kontrollera vinkelåtdragning av skruvförband

Max Thiel

Godkänt Examinator

Hans Johansson

Handledare

Bengt Eriksson

Uppdragsgivare

Atlas Copco Industrial Technique

Kontaktperson

Henrik Öhrlund

Sammanfattning

Den vanligaste metoden för att fästa komponenter till varandra vid tillverkning av produkter är genom att sammanfoga komponenterna med hjälp av skruvar, bultar och muttrar. Fördelen med denna metod är att den är billig och gör det enkelt att sammanfoga och lossa komponenter snabbt. Klämkraften i skruvförbandet är avgörande för hur väl en produkt är ihopsatt och påverkar därmed dess kvalitet, samt i viss mån livslängd. Det finns i huvudsak två olika strategier vid åtdragning i ett skruvförband – momentåtdragning och vinkelåtdragning. Den första metoden bygger på att man kontinuerligt mäter momentet under åtdragning och avbryter åtdragningen när rätt moment uppnåtts. Den andra metoden, vinkelåtdragning, mäter hur många grader fästelementet roterat i förbandet. Metoden producerar i regel högre precision med mindre spridning av den slutgiltiga klämraften. För att kunna tillämpa vinkelåtdragning med ett handhållet verktyg räcker det inte att kontroll över rotationen av verktygets utgående axel, utan även hur verktyget rör sig i förhållande till förbandet under åtdragning. Detta för att säkerställa att verktygets motorstyrning resulterar i önskad klämkraft i förbandet och inte rotation av själva verktyget.

Detta examensarbete ämnar analysera data från en IMU (Inertial Measurement Unit) integrerad i ett handhållet åtdragningsverktyg för att estimera verktygets rörelse under vinkelåtdragning och därmed kompensera för oönskade rörelser. En analys har gjorts för hur väl de två olika sensorfusions-modellerna - Kalmanfilter och Partikelfilter – presterar när det kommer till att uppskatta orientering för verktyget genom att kombinera data från IMU-enhetens accelerometrar och gyroskop.

Data samlades in från verktygets IMU från ett antal dragningar med varierande uppställning.

Tester genomfördes dels då verktyget hölls stilla under hela åtdragningen och dels då det tilläts röra sig fritt. Tester genomfördes även för flera olika orienteringar av verktyget för att i större utsträckning kunna säga hur de olika sensorfusions-modellerna presterade.

Resultatet av testerna visade att Kalmanfiltret kunde producera bättre estimeringar av verktygets orientering, speciellt i avseende precision, repeterbarhet och tillförlitlighet.

(6)

(7)

Acknowledgements

First and foremost, I would like to thank my skilled and helpful colleagues at Atlas Copco Industrial Technique for sharing their knowledge and helping me whenever possible during this thesis project. I would also like to give a special thanks to Henrik ¨Ohrlund who provided me with the opportunity to carry out this thesis project and to my industrial supervisor Wilhelm Nyberg for his unbelievable commitment and desire to guide and educate me.

I would also like to thank my supervisor Bengt Eriksson for valuable discus- sions regarding the direction of this project from a scientific point of view.

And also for his helpful and insightful comments during the finalisation of this report.

Last but not least I would like to thank my friends and family for their support throughout all my years studying at KTH. You have helped making this time into something I will cherish for years to come.

Max Thiel November 2019, Stockholm

(8)

1 Introduction 1

1.1 Overview . . . . 1

1.2 Aim . . . . 2

1.3 Scope . . . . 3

1.4 Research Question . . . . 3

1.5 Methodology . . . . 3

1.6 Challenges . . . . 4

1.7 Ethics . . . . 4

1.8 Report Outline . . . . 5

2 Frame of Reference 7 2.1 Tightening Techniques . . . . 7

2.2 Inertial Measurement Units . . . . 9

2.2.1 Error Characteristics . . . . 9

2.2.2 6-point Tumble Calibration . . . . 11

2.3 Kalman Filter . . . . 13

2.3.1 Prediction Step . . . . 14

2.3.2 Update Step . . . . 14

2.4 Particle Filter . . . . 15

2.4.1 Particle Initialization . . . . 15

2.4.2 Prediction Step . . . . 16

2.4.3 Particle Weighing . . . . 16

2.4.4 Posterior Estimate . . . . 17

2.4.5 Resampling . . . . 17

2.5 Euler Angle Representation . . . . 17

2.6 Quaternions . . . . 19

2.6.1 Rotation Using Quaternions . . . . 19

2.6.2 Quaternion Multiplication . . . . 20

2.6.3 Process Function . . . . 20

2.6.4 Measurement Function . . . . 21

2.7 State of the Art . . . . 23

(9)

2.7.1 Angle Controlled Tightening . . . . 23

3 Implementation 25 3.1 Hardware . . . . 25

3.1.1 The tool . . . . 25

3.1.2 LSM6DSM . . . . 26

3.1.3 Atmel SAM V71 Xplained Ultra . . . . 28

3.1.4 DEWE-43A . . . . 29

3.2 Communication . . . . 29

3.3 Test Setup . . . . 31

3.4 Tightening Strategy . . . . 34

3.5 Test Cases . . . . 34

3.6 Acceleration Removal . . . . 36

3.7 Extended Kalman Filter Tuning . . . . 37

3.8 Particle Filter Tuning . . . . 38

3.9 Angle from Quaternion Estimation . . . . 39

4 Results 41 4.1 Sensor Scaling . . . . 41

4.2 Static Horizontal . . . . 44

4.3 Static Vertical . . . . 46

4.4 Dynamic Horizontal . . . . 48

4.5 Dynamic Vertical . . . . 50

5 Discussion 53 5.1 Full-Scale Settings . . . . 53

5.2 Extended Kalman Filter Performance . . . . 54

5.3 Particle Filter Performance . . . . 56

5.4 Sampling Frequency . . . . 57

5.5 Test Setup . . . . 57

5.6 Conclusion . . . . 58

6 Future Work 59 6.1 Filter Implementation . . . . 59

6.2 Varying Tool and Tightening Strategy . . . . 59

6.3 Test Rig . . . . 59

6.4 Reducing the Yaw Angle Error . . . . 60

Bibliography 61

Appendix 65

(10)

B Particle Filter Code 73

(11)

List of Figures

1.1 True angle problem . . . . 2

1.2 Gantt schedule showing the preliminary planning for the thesis project. . . . 4

2.1 Angle stress-curve for tightening using threaded fasteners . . . . . 8

2.2 Brief explanation of the IMU error characteristics . . . . 10

2.3 Principle of the 6-point tumble calibration . . . . 11

2.4 General principle of the Kalman filter . . . . 14

2.5 Euler angle coordinate system . . . . 18

3.1 Atlas Copco angle nutrunner . . . . 26

3.2 Position and coordinate axes of the IMU . . . . 26

3.3 SAM V71 Xplained Ultra . . . . 28

3.4 DEWE-43A . . . . 29

3.5 Protocol structure . . . . 29

3.6 Example of task allocation to scheduler slots. . . . 30

3.7 Software communication from the IMU to PC . . . . 31

3.8 Beam joint . . . . 32

3.9 Test setup . . . . 32

3.10 Angle, α, calculation from laser measurements . . . . 33

3.11 Visualization of static and dynamic test cases . . . . 35

3.12 Visualization of horizontal and vertical test cases . . . . 35

3.13 IMU coordinate axes within the tool . . . . 36

3.14 Angle between two orientations . . . . 40

4.1 Full-scale settings, Accelerometer: 16g, Gyroscope: 2000dps . . . 42

4.2 Full-scale settings, Accelerometer: 8g, Gyroscope: 1000dps . . . . 42

4.3 Full-scale settings, Accelerometer: 2g, Gyroscope: 500dps . . . . . 43

4.4 Full-scale settings, Accelerometer: 2g, Gyroscope: 250dps . . . . . 43

4.5 Static Horizontal, test no.1 . . . . 45

(12)

4.9 Static Vertical, test no.2 . . . . 47

4.10 Static Vertical, test no.3 . . . . 48

4.11 Dynamic Horizontal, test no.1 . . . . 49

4.14 Dynamic Vertical, test no.1 . . . . 51

5.1 Accelerometer raw data from one tightening. . . . 54 5.2 Average and down-sampled accelerometer data from one tightening. 55

(13)

List of Tables

2.1 6-point Tumble Calibration Reference Values . . . . 12

3.1 Full-scale settings of the LSM6DSM . . . . 27

3.2 Sampling rates of the LSM6DSM . . . . 28

4.1 RMSE Static Horizontal . . . . 44

4.2 RMSE Static Vertical . . . . 46

4.3 RMSE Dynamic Horizontal Movement . . . . 48

4.4 RMSE Dynamic Vertical Movement . . . . 50

(14)

Acronyms

DoF Degree of freedom P F Particle Filter

CAN Controller Area Network EKF Extended Kalman Filter IMU Inertial Measurement Unit IMU Inertial Measurement Unit KF Kalman Filter

MCU Microcontroller Unit

MEMS Microeletromechanical Systems RMSE Root Mean Square Error SOTA State of the Art

SPI Serial Peripheral Interface

UART Universal Asynchronous Receiver Transmitter UKF Unscented Kalman Filter

Symbols

ω Angular velocity

φ Roll

ψ Yaw

θ Pitch

(15)

Chapter 1 Introduction

This chapter aims to presents an overview on the use of handheld tightening tools in industrial applications today. The problem formulation, scope and outline of this report are also presented.

1.1 Overview

There are multiple methods for securing components to each other. However, the most common method, by far, is using screws and bolts to press components together. The benefit of using screws and bolts, more generally referred to as threaded fasteners, is that this solution is cheap, easy to use and allow for fast assembly and disassembly of components. The force joining the components is called clamping force and determines both the quality and lifetime of a product. In order to ensure the quality of a product while minimizing its weight and cost, the assembly should only use properly sized fasteners of a reasonable amount. In tightening technique and assembly industry it is therefore of high importance that the clamping force in each joint is sufficient and can be achieved with high certainty. This is particularly true for applications in aerospace and automotive industries where the final products have to withstand large variations in weather conditions and external forces while remaining to function safely throughout is its relatively long lifetime. This in combination with increasing environmental demands requiring lighter and more sustainable products result in an increasing demand of better controlling the clamping force.

(16)

1.2 Aim

This thesis project was proposed by Atlas Copco Industrial Technique - a world leading developer and manufacturer of a wide range of tightening tools.

In order to achieve better tightening results when using hand-held tightening tools, Atlas Copco expressed a desire to analyze tool movement during tightening processes. The proposed method was to perform an analysis based on measurement data recorded from an inertial measurement unit (IMU) located within the tool. After talking to engineers at Atlas Copco and having performed the initial information search on existing tightening techniques and IMU applications, the aim of this thesis project was decided upon in collaboration with the industrial supervisor. The conclusion was that an analysis would be performed regarding the possibility to estimate tool movement during an angle tightening process. Angle tightening, also called ”turn-of-nut”, is the principle of turning the threaded fastener a certain number of degrees in order to get a very precise clamping force. The principle of angle tightening is further presented in section 2.1. Since the tightening process is performed using a hand-held tool operated by a person the true tightening angle might differ from time to time due to reaction forces in the joint. The basic problem is illustrated below in figure 1.1.

Figure 1.1: True angle problem

In conclusion, the aim of this thesis project is to analyze IMU data and evaluate existing signal processing methods in an attempt to reduce the effects of sensor errors and noise, such as mechanical vibrations and electrical interferences, when estimating the orientation of a hand-held tightening tool.

(17)

1.3. SCOPE

1.3 Scope

The angle tightening is to be performed on a hand-held battery-driven tool.

Since the tool will not be connected directly to a computer the handling of motor control and IMU data will have to be performed using the on-board microcontroller. This concludes that signal processing algorithms will have to be real-time compatible and possible to run on an embedded system with limited computational power. To simplify the design and testing of system model and filters, these will be developed and managed in Matlab. This in turn requires the IMU data to be recorded from the IMU within the tool for a series of test cases before evaluation of filter behaviour can be performed.

1.4 Research Question

With regard to the above stated background, aim and scope of the project this thesis project will aim to answer the following question:

”Given the PID control strategy, what are the implications of the signal processing algorithms, Kalman Filter or Particle Filter, in regard to pro- viding accuracy and repeatability when controlling the angle tightening process using a hand-held tightening tool in an industrial environment?”

1.5 Methodology

The initial phase of the project will focused on performing a literature re- view with emphasis on finding conducted projects relating to this thesis and exploring different filtering techniques. The proposed filtering technique was evaluated in terms of real-time capabilities, high precision in the state estimate and needed be suitable for implementation on an embedded device. The next phase of the project was initiated with formulating a research question, see section 1.4, which was followed by designing early versions of system model and filter algorithms. Following the creation of system model and filters, test data needed to be recorded from a tool. This required the creation of a state- of-the-art communication setup, presented in section 3.2, which was developed in collaboration with the industrial supervisor from Atlas Copco. The results of filter performances were gathered and evaluated in a quantitative manner for a number of different test cases. An overview of the planning for this project can be seen below in figure 1.2.

(18)

Figure 1.2: Gantt schedule showing the preliminary planning for the thesis project.

1.6 Challenges

This thesis project poses some interesting challenges that comes with signal processing in an industrial environment. First and foremost, the signal processing has be effective for different tools, operating conditions and tightening strategies. The proposed method should ideally suit any configuration, or at least be modular to enough to allow for simple setup without compromising the results.

Due to minor differences during manufacturing of each IMU as well as assembly of each tool the sensor output might differ from tool to tool. And since calibration is a time consuming subject it is desired that the signal processing method can cope with these uncertainties.

1.7 Ethics

The proposed method has to produce accurate and repeatable results when being used to control angle tightening processes. It is therefore highly important that the results presented in this thesis are truthful and have an open mind towards potential shortcomings. Especially considering that the major stakeholders that require high precision clamping force are the automotive and aerospace industries. The proposed method should therefore also be exposed to the worst possible scenarios that could occur in an industrial environment before being deployed.

Another ethical aspect is operator safety. The proposed method should be stable and reliable enough to spot potential scenarios which might compromise operator safety.

(19)

1.8. REPORT OUTLINE

1.8 Report Outline

The report is composed of six chapters. The first chapter provides an introduction to the topic, problem formulation and scope of the project. In the second chapter theory concerning this thesis project is presented including a brief presentation of previous works relating to this thesis project. The third chapter presents an overview of the hardware and software used. Chapter three also presents how system models and filter were set up and how test were conducted. In chapter four the results are presented which is followed by a discussion in chapter five. Chapter six presents a discussion on future im- provements to the test setup and filter algorithms that could not be performed due to hardware restrictions or lack of time.

(20)

(21)

Chapter 2 Frame of Reference

This chapter aims to present background and theory relating to the topic of tightening techniques, inertial measurement units, signal processing and ori- entation estimation.

2.1 Tightening Techniques

There are two main types of tightening control strategies where the most common method is torque controlled tightening[1]. This type of tightening control strategy makes use of the possibility measure the torque during a tightening process, through either static or dynamic measurement. Static measurement can be performed using a torque wrench on a tightened bolt.

If the torque is greater than the setting of the torque wrench, the wrench will click. If the torque is less the torque wrench can be used to do the final tightening. Dynamic measurement often uses a torque transducer to measure the torque directly, or indirectly by measuring the used current of the tool. Torque transducers can either be fitted inside the tool or as a separate unit attached to the tool. Dynamic measurements have the advantage of not needing to measure the torque after tightening, which can be time consuming in the long run. Using torque measurements to control the tightening process is cheap and generally provides good results for many applications. When controlling the clamping force in a joint using torque measurements, only a fraction of the applied torque is transferred to clamping force[2]. The rest of the applied torque is lost in the form of friction against the screw head and meshing of the threads, meaning that the target torque when performing a torque controlled tightening is greatly affected by the amount of lubrication as well as surface roughness[3][4][5]. This results in the scatter of clamping force being relatively high for torque controlled tightening[6].

(22)

The other main tightening control strategy is called angle controlled tightening and serves the purpose of tightening the bolt/screw a certain number of degrees to reach the desired clamp force. The clamping force in the joint is proportional to the elongation of the screw and thereby the angular dis- placement of the threaded fastener. This relationship is presented in figure 2.1.

Figure 2.1: Angle stress-curve for tightening using threaded fasteners[2]

Angle controlled tightening can generally produce more accurate results with less scatter in the clamping force as opposed to torque controlled tightening[7][8][9].

Since angle controlled tightening can provide more accurate results this has paved the way for tightening over the yield point of the bolt - something that is common when the clamping force requirements are very strict[2][10]. The angle controlled tightening process can be monitored by calculating the number of revolutions in the motor of the tool after the the snug point has been reached. The snug point can be defined as the moment when the surface of the bolt comes into contact with the work piece, which is detected by a notable sudden increase of torque.

Combining torque and angle control when tightening a threaded joint can also help spot errors in assembly, for example missing fastener components such as gaskets or washers[2].

(23)

2.2. INERTIAL MEASUREMENT UNITS

2.2 Inertial Measurement Units

An inertial measurement unit (IMU) usually contains multiple accelerometers, gyroscopes and sometimes magnetometers [11]. The working principle of an IMU is that accelerometers measure linear acceleration, and gyroscopes measures angular rates [12] along multiple axes which can be used to estimate the orientation and movement of an object. In the case the IMU is equipped with a three-axis magnetometer it is also possible to estimate absolute heading direction by measuring the earths magnetic field.

IMUs have throughout history mostly been used in aerospace and automotive applications. These industries have a requirement of maintaining high stability and accuracy over longer periods of time resulting in the sensors having large dimensions, being heavy and a high cost[13][14]. This reduced the number of applications that IMUs could be used for until development of micro-eletromechanical systems (MEMS) IMUs. MEMS can be defined as ”a functional unit that contains electrical and/or mechanical ’components’ with characteristic sizes ranging from 100nm to 1mm”[15]. With sensors becoming smaller, lighter and cheaper, MEMS IMUs have become more and more common in consumer products such as mobile phones, cameras, drones and fitness clocks to name a few[12]. But with the smaller size and lower unit price, the output signals of MEMS sensors suffer from greater errors and noise[16][17].

Applications that rely on IMU data to estimate position and/or orientation require both high precision and resolution in the sensor data. The cause for this is that angular rates, sensed by the gyroscopes, to be integrated once in order to get an orientation change and acceleration, sensed by the accelerometers, to be integrated twice to get a position. This means that a constant error in gyroscope measurements will grow linearly over time and a constant error in accelerometer measurements will grow quadratically[18], causing the system to drift uncontrollably[19][20]. In order to reduce the errors, sensors need to be calibrated and carefully designed sensor fusion algorithms need to be implemented. The most common error characteristics of MEMS IMUs are further presented in section 2.2.1. In section 2.2.2 a calibration method is presented to remove biases and scale errors in accelerometer measurements.

2.2.1 Error Characteristics

As with most sensors, IMUs have several possible error sources. And in order to accurately handle data from the sensor it is important to understand why these errors exist and how they occur in order to counteract for them. This section will present some of the most common error characteristics of both

(24)

gyroscopes and accelerometers. Figure 2.2 below aims to illustrate the most common error sources.

Figure 2.2: Brief explanation of the IMU error characteristics[21]

Bias: Bias is an offset in the the output of the sensor. The bias in the sensor can be different at every start-up of the IMU due to physical properties.

Roughly the bias can be estimated by comparing the sensor value to a reference. For example the gyroscopes should not output any rotational rate during static operation. When a good estimation of the bias has been made it can be subtracted from the sensor output to obtain a better result.

Scale: Scale factor is the relation of the sensor input compared to sensor output. Due to factory calibration the scaling of the sensor output might not be accurate and often requires the IMU to be re-calibrated. Although The scale factor is normally linear but can also include non-linear characteristics.

Temperature: Due to physical properties of the IMU the performance and outputs of the sensor are affected by the surrounding temperature[22]. Since the IMU operating environment most likely won’t be the same for one work cycle or even parts of a work cycle this will lead to errors in terms of both bias and scale. Considering the changing environment of the IMU, this error source is troublesome to counteract for relying only on calibration of the IMU.

Instead some IMUs are equipped with a temperature sensor in to overcome this shortcoming.

White noise: Even in static cases, where the output of the IMU should be constant, there are always fluctuations in the output. This is due to random white noise and one way to minimize this error is to use filtering and statistical

(25)

2.2. INERTIAL MEASUREMENT UNITS

techniques[21]. White noise can also occur due to electrical interferences from nearby electrical components.

Non-Orthogonality: The axes of the IMU are not mutually orthogonal.

This is an error that can originate from either manufacturing or integration/assembly of the IMU in a tool. This means that in the case that one axis is pointed directly upwards, and would thus measure gravity, the other axes would ideally measure 0. But misalignment of the axes would amount to that the IMU would sense some of the gravity on another axis - a correlation between measurements. This issue can also be resolved through calibration techniques.

2.2.2 6-point Tumble Calibration

Accurate estimation of orientation using sensor fusion requires precise data from accelerometers and gyroscopes. Despite some sensors being calibrated as the final step of manufacturing the sensor might need to be re-calibrated before being used. The 6-point calibration method is a way of estimating biases and scaling the output signal of three-axis accelerometers and can additionally reduce the effect of non-orthogonality[23]. The working principle of the 6- point calibration method is to position the IMU in six distinct positions and combine measurements to solve a system of equations to acquire biases and scale factors. The positions can be seen in figure 2.3 below.

Figure 2.3: Principle of the 6-point tumble calibration

(26)

From these six positions the accelerometers should ideally output values according to table 2.1.

Table 2.1: 6-point Tumble Calibration Reference Values Number Position Reference

1 X_up [ 1 0 0 ]

2 Xdown [−1 0 0 ]

3 Y_up [ 0 1 0 ]

4 Y_down [ 0 −1 0 ]

5 Z_up [ 0 0 1 ]

6 Z_down [ 0 0 −1 ]

The relationship between acceleration sensed by accelerometers and reference accelerations is described by equation 2.1.







x_acc y_acc z_acc





=







x_gain y2x z2x x2y ygain z2y x2z y2z zgain













x_ref y_ref z_ref





+







x_bias y_bias z_bias





 (2.1)

Here xacc denotes the acceleration sensed by the sensor, xgain denotes the scale factor of the accelerometer sensing acceleration along the x-axis in the sensor frame, y2x is the cross-gain due to misalignment, xref is the reference acceleration along the x-axis and xbias is the bias in the sensor. The biases are calculated by adding pairs of measurements with opposite gravitational direction, i.e. Xup and Xdown. Combining the two measurements results in the following system of equations.

x_acc,1+ xacc,2= 2xbias

yacc,1+ yacc,2= 2ybias

zacc,1+ zacc,2= 2zbias

(2.2)

Similarly, by combining pairs of measurements of the remaining data, addi- tional estimations can be calculated and the final estimation of the biases is obtained by calculating the average of the three sets of biases. The gains and cross-gains can now be calculated by inserting one reference, seen in table 2.1, at a time and solve for unknown variables.

(27)

2.3. KALMAN FILTER

When all calibration parameters are calculated the measured accelerometer data can be converted to the calibrated acceleration using equation 2.3.







x_calibrated y_calibrated z_calibrated





=







x_gain y2x z2x x2y ygain z2y x2z y2z zgain







−1











x_acc y_acc z_acc





−







x_bias y_bias z_bias











 (2.3)

2.3 Kalman Filter

There are a number of different ways of implementing signal processing of IMU data. The Kalman Filter (KF) , first introduced by Rudolf E. Kalman in 1963, is an optimal estimation algorithm that can be applied to linear systems where measurement and process noise is present. The KF has been applied in a wide range of different applications due to the algorithm being recur- sive, effective in minimizing estimation error and having real-time capabilities [24]. These characteristics have made the KF popular in applications covering target tracking, localization and image processing. The real-time capabilities also make the KF suitable for implementation on microprocessors, as seen in [25] where a KF used measurements from an IMU to balance a two-wheeled robot.

The KF is only applicable to linear systems and since most real systems are nonlinear there are other variations of the filter to handle this issue. There are two other major versions of the KF that can handle non-linear systems.

The first is called the Extended Kalman Filter (EKF) and uses a first order linearization by constructing Jacobians of the process and measurement matrices [24][26]. Both the KF and EKF use one point at a time to estimate system behaviour. The principle behind the third major version of the KF, the Unscented Kalman Filter (UKF) is to use a set of so-called sigma points to find a better estimation for systems with highly non-linear behaviour[27]. One thing however that all versions of the KF have in common is that all models assume the process and measurement noise to take the form of a Gaussian distribution.

In this thesis the system is non-linear and hence we will focus on describing the EKF in closer detail. In the subsections that follow a universal EKF algorithm is presented. In general the algorithm consist of two steps, the prediction step and the update step. A priori estimate of system states is made in the prediction step and the states are corrected in the update step using the system measurement function. In figure 2.4 the principle of the KF can be seen.

(28)

Figure 2.4: General principle of the Kalman filter

2.3.1 Prediction Step

A priori estimate of system states ˆxk|k−1, with subscript k denoting the current time step and k−1 the previous time step, are predicted using the last state estimate, ˆxk−1, and system inputs uk and the process function f

ˆ

x_k|k−1 = f(ˆxk−1, u_k) (2.4)

The prior covariance estimation is calculated using the state transition matrix F_k, the last estimated covariance Pk−1 and the process covariance Q

P_k|k−1= FkP_k−1F^T_k + Q (2.5)

2.3.2 Update Step

The difference between the measured variables zk and the measurement func- tion h using the priori estimate ˆxk|k−1 is calculated and results in the mea- surement residual ˜yk[28]

˜

y_k= zk− h(ˆxk|k−1) (2.6)

The residual covariance Sk is calculated using the measurement Jacobian Hk, the covariance matrix Pk|k−1 and the measurement covariance R

S_k = HkP_k|k−1H^T_k + R (2.7)

(29)

2.4. PARTICLE FILTER

The Kalman gain is calculated from priori estimate of the covariance Pk|k−1, the measurement Jacobian H^Tk and the inverse of the residual covariance S⁻¹k

K_k = Pk|k−1H^T_kS⁻¹_k (2.8)

Calculate the posterior state estimate using the priori state estimation, measurement residual and the Kalman gain

ˆ

x_k = ˆxk|k−1+ Kk˜y_k (2.9)

Update the covariance estimate

P_k = (I − KkH_k)Pk|k−1 (2.10)

2.4 Particle Filter

Another type of filter, called the Particle Filter (PF), has gained a lot of popularity in recent years[29][30]. One of the benefits of the PF is that it can handle highly non-linear and non-Gaussian models much better than, for example, versions of the KF. These attributes of the PF has also proven it to be more robust than other common signal processing methods [31]. The main principle of the PF is to introduce a number of particles, where every particle poses a state estimate. The particles are then given a weight according to the likelihood of that particle posing a correct estimate. Lastly the particles are re-sampled with respect to their weights. In order to evaluate the likelihood of a particle the PF often uses knowledge of its surroundings, i.e. a map. This makes the PF suitable for the problem of robot localization within buildings or for underwater terrain navigation [32]. Another example of an application of the PF is presented in [33] where a version of the PF is used to determine the position of a person walking inside a building. The above mentioned method proved to work well when estimating the position of a person within a building, even without compensating for IMU sensor biases.

The PF has some main drawbacks that include high computational demand depending on the number of particles used[29]. Another drawback for the PF is that the performance is restricted to low-dimensional state spaces [34][35].

2.4.1 Particle Initialization

Depending on the problem the initialization of particles can be performed differently. Consider for example the task of localization within a building

(30)

where there is little to no knowledge of the current position. Then the particles could be initialized to cover the entire floor plan. In other applications there may be some knowledge in regards to the initial state and particles could in that case be distributed around the best estimate. The third option is when there is absolute knowledge of the initial states of the system. Here all particles are assigned the same value.

During initiation all particles are assigned the same weight.

2.4.2 Prediction Step

Similar to the prediction step of the EKF, see section 2.3.1, the prediction step uses the system process function f(ˆxk−1, u_k) and system input ukto find a priori estimate of the system states ˆxk|k−1. The update step is performed for each particle. All system states are however not updated with using the same input uk, but rather with an input drawn from a likely distribution. In this thesis we will assume that the input to the system is drawn from normal distribution with standard deviation σu

u_k∼ N(uk, σ_u²) (2.11)

2.4.3 Particle Weighing

The weighing of each particle is performed by comparing the particle to the measured variables zk. The total weight of a particle is calculated using Bayes theorem where the previous weight of the particle is multiplied by the calculated weight of the current prior estimation of the particle. The weight of the current estimation can be calculated as follows:

w_k,i= wk−1,i

1

q2πσv²

e⁻

(zk−h(ˆxk,i|k−1,i))2

σ2v (2.12)

Here, the subscript k denotes the current time step, and subscript i the particle number. Lastly the weights are normalized

w_k,i= w_k,i

PN

i=1w_k,i (2.13)

(31)

2.5. EULER ANGLE REPRESENTATION

2.4.4 Posterior Estimate

The posterior estimate of the PF is calculated using the particles and their corresponding weight to form a weighted average.

ˆ

x_k=^X^N

i=1

w_k,i ˆx_k,i|k−1,i (2.14)

2.4.5 Resampling

One very important step of the PF was the introduction of the resampling step where particles with low weight are disregarded in favour for more likely ones.

This to avoid particle degradation and divergence in the state estimation[29].

There are a number different resampling methods were the most common perhaps is Systematic Resampling. This algorithm is both computationally effective and produce good particle estimates[36][37]. This algorithm calcu- lates the cumulative sum of the normalized weights. This creates an array with different-sized intervals giving particles with large weights a larger interval and thus a higher likelihood of being re-sampled.

2.5 Euler Angle Representation

Perhaps the most intuitive way of thinking about the orientation of a solid object is describing it using Euler angles. The three-dimensional orientation of a solid object can be expressed in terms of roll (φ), pitch, (θ) and yaw (ψ) angles which explain the rotation around the x-, y- and z-axes respectively, as seen in figure 2.5.

(32)

Figure 2.5: Euler angle coordinate system

The rotation around each axis is expressed in matrix-form as:

R_x(φ) =







1 0 0

0 cos(φ) −sin(φ) 0 sin(φ) cos(φ)





 (2.15)

R_y(θ) =







cos(θ) 0 sin(θ)

0 1 0

−sin(θ) 0 cos(θ)





 (2.16)

R_z(ψ) =







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

0 0 1





 (2.17)

The combined rotation can be expressed as product of the three matrices R(φ, θ, ψ) = Rx(φ)Ry(θ)Rz(ψ) (2.18) However there is a problem using Euler angles when the pitch angle is ±90^◦ called gimbal lock [38][39]. This causes the system to lose one degree of freedom (DoF) and thereby a poor estimation of orientation.

(33)

2.6. QUATERNIONS

2.6 Quaternions

Quaternions is another method for describing rotations in 3D-space. The quaternion was most famously introduced by Irish mathematician William Rowan Hamilton who had long searched for a way to represent translation and rotation of points in 3D-space. Hamilton knew that complex numbers could be used to describe translation and orientation in a two-dimensional plane. And while addition and subtraction could be performed in the same way in three dimensional space, Hamilton was stuck on the principle of how to handle rotations for several years. But then one day in 1843 Hamilton, during a walk, came up with the mathematical formulation that marked the start of the quaternion

i² = j² = k² = ijk = −1 (2.19)

2.6.1 Rotation Using Quaternions

With help from this brainwave Hamilton realised that a fourth dimension was needed in order to handle rotations in three-dimensional space, hence the name quaternion. A quaternion, q, describing the orientation of frame B in relation to frame A is denoted with left subscript B and superscript A is composed of four parts

A

Bq= q0+ q1i+ q2j+ q3k (2.20) From equation 2.20 it can be see that the quaternion consists of a real part and three imaginary parts. The real part q0 is often referred to as the scalar part whereas the imaginary parts are referred to as the vector part. As with complex numbers the quaternion conjugate, q^∗, is a quaternion with the same scalar part but with opposite sign of the vector part.

A

Bq^∗ = q0 − q1i − q2j − q3k =^BAq (2.21) As seen from equation 2.21 the quaternion conjugate is also used to express the opposite rotation between two frames, i.e. the orientation of frame A in relation to to frame B.

When the dealing with pure rotations of a solid object the best practise is to use a normalized quaternion, i.e. the unit quaternion, which has the following property

q²₀ + q1²+ q2²+ q3² = 1 (2.22)

(34)

2.6.2 Quaternion Multiplication

Returning to equation 2.19 it might seem unclear as to why ijk = −1. This is a feature that comes from the definition for quaternion multiplication, sum- marized as

ij = k;

ik= −j;

ji= −k;

jk = i;

ki= j;

kj = −i

(2.23)

Quaternion multiplication, denoted by ⊗, is non-commutative, i.e.

q_A⊗ q_B 6= qB⊗ q_A (2.24)

Although using the quaternion representation for rotations of a rigid body may seem unintuitive, it possesses some advantages over the more intuitive Euler representation:

• Not subject to any singularities (gimbal lock) [38][39];

• It is more computationally effective

2.6.3 Process Function

In previous sections the mathematical foundations of the quaternion has been presented. In this section focus will be on explaining how the quaternion representation can be used to construct a system state space model using IMU data to update system states. When dealing with pure rotation, the system states are the quaternion components, namely: [q0 q₁ q₂ q₃]^T.

The IMU senses angular rate along three axes which can be used to estimate system states. Using gyroscope measurements system states can be updated by constructing the quaternion derivative, formulated in equation 2.25

˙ q= 1

2q ⊗^Sω (2.25)

Sω is a vector containing angular rates sensed by the IMU gyroscopes. The left superscript denotes that the angular rates are expressed in the sensor

(35)

2.6. QUATERNIONS

frame.

Sω= [0 ωx ω_y ω_z] (2.26)

The quaternion product in equation 2.25 can be replaced by

˙

q =^SEΩq = 1 2







0 −ωx −ω_y −ω_z ω_x 0 ω_z −ω_y ω_y −ω_z 0 ω_x ωz ωy −ωx 0







q (2.27)

to enable the system to be updated using matrix multiplication instead of quaternion multiplication. From equations 2.25-2.27 the process equation used to estimate quaternion state variables is

q_k = f(qk−1, ω_k) = (I +^SEΩ∆t)qk−1 (2.28) From the equation above the process Jacobian Fk can be calculated using partial integration.

F_k = δf δq

_q

k|k−1

= I +^SEΩ∆t (2.29)

2.6.4 Measurement Function

Using the angular rates sensed by the IMU, system states can be updated using the process function described in the previous section. Small errors in gyroscope values might however result in a poor estimation of system states over time. This issue is overcome by taking advantage of the fact that IMU accelerometers can sense the gravitational direction, assuming that the system is kept at a constant velocity, i.e. not subject to any acceleration. Since measuring the direction of gravity can be done directly, measurements from the accelerometers can be used to construct a measurement function for the system.

The principle of the measurement function is to compare the accelerations sensed by the IMU, ^Sa = [0 ax a_y a_z]^T, to the gravity vector, ^Ev_g = [0 0 1]^T. But in order to compare the the two they have to be expressed in the same coordinate frame. In general a vector described in the earth frame, E, can be rotated to the sensor frame, S, using the equation below.

Sv=ÊSq ⊗Êv ⊗Ê_Sq^∗ (2.30)