Calibration and Evaluation of Inertial Navigation Using Zero Velocity Update for Industrial Tools

(1)

IN

DEGREE PROJECT ELECTRICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

,

STOCKHOLM SWEDEN 2021

Calibration and Evaluation of Inertial Navigation Using Zero

Velocity Update for Industrial Tools

JOHAN RÅGMARK

KTH ROYAL INSTITUTE OF TECHNOLOGY

SCHOOL OF ELECTRICAL ENGINEERING AND COMPUTER SCIENCE

(2)

JOHAN RÅGMARK

Date: February 24, 2021

Supervisor: Carl-Mikael Zetterling Examiner: Mark Smith

School of Electrical Engineering and Computer Science Host company: Atlas Copco

Swedish title: Kalibrering och Evaluering av Tröghetsnavigering Användandes Zero Velocity Update för Industriverktyg

(3)

Calibration and Evaluation of Inertial Navigation with Zero Velocity Update for Industrial Fastening Tools / Kalibrering och

Evaluering av Tröghetsnavigering Användandes Zero Velocity Update för Industriverktyg

(4)

noisy Inertial Measurement Unit (IMU) sensors which contribute to large drift. Current IPN systems usually involve the installation and calibration of cameras or antennas, so achieving sufficient accuracy with inertial navigation based IPN would be very desirable. This project aims to evaluate an inertial navigation algorithm, based on Zero Velocity Update (ZUPT), for bolt level positioning by repeatability tests using an industrial robot.

The ZUPT algorithm, developed at Atlas Copco, manages to effectively reduce drift and achieve moderate accuracy in position for simpler movements.

The gravity tracking Kalman filter dictates the systematic errors in position that grow large with increased degree and dimension of rotation. When keeping rotations within 45^◦ for a linear movement the absolute error in position is under 10%. Frequent stops are important when moving in a more complex trajectory to be able to negate drift, consequently detecting the start and stop of motion is crucial. The results show that increased frequency will improve accuracy. It is shown that averaging IMU samples before calculations can increase both truthfulness and precision by10−25%, if sampling the IMU faster than the calculations.

The ZUPT approach of inertial navigation will never yield positional results in real time, and the evaluated algorithm only performs well within certain limitations, mainly frequent stops and simple movements. Despite these limitations there is potential in using the algorithm for quality assurance purposes in hand held industrial fasteners.

Keywords

Inertial navigation, Inertial measurement unit, Zero velocity update, Indoor positional navigation, Kalman filter

(5)

iv | Abstract

(6)

har centrala styrsystem som kommunicerar med maskiner och verktyg, och ifall något blir fel är det vanligt att fabrikslinan stannar vilket blir kostsamt.

Inomhuspositionering (IPS) av hög noggrannhet kan spåra vilken åtdragning som blivit fel, vilket dokumenteras och åtgärdas om möjligt senare, utan att stanna fabrikslinan.

Dagens noggranna IPS system för kvalitetssäkring kräver installation och kalibrering av kameror och/eller antenner. Tröghetsnavigering kräver i grunden bara billiga sensorer installerade på verktyget men metoden är mycket opålitlig på grund av sensorernas opålitlighet och brus. I detta projekt har en metod för tröghetsnavigering, användandes Zero Velocity Update (ZUPT), evaluerats för kvalitetssäkring av handhållna verktyg genom repetabilitetstester.

Tröghetsnavigeringsalgoritmen som tidigare utvecklats på Atlas Copco lyckas på effektivt sätt reducera drift och uppnår rimlig noggranhet för enklare rörelser. För linjära rörelser med rotationer under 45^◦ så erhålls ett absolut positionsfel inom 10%. För att fungera väl även för mer komplexa rörelser krävs frekventa stop, och noggrann rörelsedetektion är central.

Denna ZUPT-metod kommer aldrig att kunna generera position i realtid och algoritmen presterar väl endast inom vissa begränsningar. Trots detta så finns god potential för metoden inom kvalitetssäkring för handhållna industriverktyg.

Nyckelord

Tröghetsnavigering, Inertial measurement unit, Zero velocity update, Inomhuspositionering, Kalman filter

(7)

vi | Abstract

(8)

the project and helpful comments. I would also like to thank my supervisors at KTH, Mark Smith and Carl-Mikael Zetterling, who were happy to take me on and helped me form a great thesis. I am also grateful for the support from the Total Workstation team at Atlas Copco, mainly my supervisor Stefan Olofsson.

Lastly, I am immensely grateful to my family who has supported me throughout the long and difficult studies.

Stockholm, February 2021 Johan Rågmark

(9)

viii | Acknowledgments

(10)

List of Figures

1.1 One of Atlas Copco’s electric angle nutrunner tools [1]. . . 3 2.1 Coordinate frames and rotations. . . 8 2.2 Difference between precision, trueness and accuracy: (a) high

precision, low trueness (b) high trueness, low precision (c) high accuracy, both high trueness and precision. . . 12 2.3 Demonstration of principal component analysis on a set of data. 14 2.4 Demonstration of noise, offset and scale factor. . . 15 2.5 Configurations of the 6-point tumble calibration.. . . 18 2.6 Block diagram of positioning algorithm developed at Atlas

Copco. There are two main blocks: the filter module (upper, light grey) and the integration module (lower, light yellow).

The filter module calculates the tilt compensated dynamic accelerations, which can be integrated directly into position.

The integration module applies Zero velocity Update (ZUPT), motion detection and velocity drift remover, before integrating into the resulting position. . . 21 2.7 Principle of velocity drift remover. The algorithm fits a linear

curve on calculated velocity between the detected start of movement and end of movement and subtracts it from the velocity. . . 25 3.1 Measurement and configuration set-up for the tool. . . 28 3.2 The UR5 industrial robot with the nutrunner tool mounted.

The robot interface is shown in the bottom of the image. . . . 29 3.3 Visualization of motion detection of handheld linear motion

with rotation of the tool. Figure of motion detection parameters and their thresholds in the top and the binary detection curves are shown in the lower figure. . . 35

(15)

xiv | LIST OF FIGURES

4.1 Output from accelerometer and gyroscope for violent shaking and rotation for each sensitivity setting. . . 38 4.2 Comparison in time domain of output from the tool when

setting the low-pass cutoff frequency to its next lowest and the highest setting, in left and right column respectively. . . 38 4.3 Difference in calculated position when using ZUPT and velocity

drift remover. The raw data is from moving the IMU in a square (with rotation in one axis) with stops of 1 s in each corner. (a) is the position calculated when using motion detection (ZUPT) and velocity drift remover (b) is the position calculated when using motion detection (ZUPT) (c) is the position calculated from the raw velocity data. . . 40 4.4 Positional calculations for 30 cm linear movement inx with

varying degree of roll and yaw rotation of 30^◦, 60^◦ and 90^◦ degree. The trajectory is the mean of 20 measurements and the error bars correspond to the± absolute standard deviation, equation 3.1. . . 41 4.5 Positional calculations for 30 cm linear movement inx with

varying degree of simultaneous roll and negative yaw rotation of 30^◦, 60^◦ and 90^◦ degree. . . 42 4.6 Positional calculations for 30 cm linear movement inx with

varying degree of simultaneous roll, yaw and pitch rotation of 30^◦, 60^◦and 90^◦degree. . . 43 4.7 Square with one dimensional rotation and varying frequency

of stops. Ten measurements of the same robot controlled motion (a) stop in each corner (b) stop in every other corner (c) stop only at end position. . . 44 4.8 Square with three dimensional rotation and varying frequency

of stops. (a) stop in each corner (b) stop in every other corner (c) stop only at end position. . . 44 4.9 Square movement orthogonal to gravity (a) without rotation

(b) with rotation around Z (c) with rotation around Z and X (d) rotation around all axes. Ten repetitions of the same robot controlled movement are shown. . . 44 4.10 Square motion with 40 cm side length and stop in each corner. 46 4.11 Square motion with 2 cm side length without rotation. . . 46 4.12 Difference in position calculation when altering gravity tracking

parameters for a square movement with 3D rotation, (a) represents the parameters used for the other calculations. . . 47

(16)

3.1. . . 50 4.15 Impact of calibration. Top row are calculated square movements

when not compensating the raw IMU data with the offset and scale factor calibration. The bottom row are calculated square movements with compensated offset but 3% added scale factor error.. . . 51

(17)

xvi | LIST OF FIGURES

(18)

List of Tables

3.1 Low-pass filter cut-off frequencies . . . 30 4.1 Chosen settings for ISM330DLC. . . 38 4.2 Accelerometer calibration parameters. The number of counts

is multiplied by0.122 × 10⁻³to convert tom/s²[2]. . . 39 4.3 Gyroscope calibration parameters. The number of counts is

multiplied by 0.0175 to convert into dps [2]. . . 39 4.4 Resulting truthfulness and precision for each of the four corner

positions in the square pattern when stopping at each corner.

Percentage is calculated as the absolute error in position or standard deviation divided by the side length of the square. . . 45

(19)

xviii | LIST OF TABLES

(20)

dps degrees per second EKF Extended Kalman Filter

GNSS Global Navigation Satellite Systems GPS Global Positioning System

ILT Industrial Location Tethering IMU Inertial Measurement Unit IPN Indoor Positional Navigation IR Infrared

ISO International Organization for Standardization MCU Micro-Controller Unit

MEMS Microelectromechanical System NLOS Non-Line-Of-Sight

ODR Original Data Rate

PCA Principal Component Analysis SiP System-in-Package

TCP Tool Center Point

UAV Unmanned Aerial Vehicle UWB Ultra Wide Bandwidth ZUPT Zero velocity Update

(21)

xx | List of acronyms and abbreviations

(22)

Chapter 1 Introduction

Tracking the geographical position of objects is important in many applications and has become an integral part of quality assurance in many modern factories.

Indoor Positional Navigation (IPN) systems come with a different set of challenges compared to tracking position outdoors since Global Positioning System (GPS)triangulation can not be used. One popular method is to track an objects movements through measuring accelerations and rotations which can be done using anInertial Measurement Unit (IMU). TheIMUis a cheap and widely available component but it suffers from noise and offsets making it difficult to use without careful calibration and filtration. It is therefore mainly used in conjunction with other positioning systems through sensor fusion to lessen the issues that external systems might experience from e.g. Non-Line- Of-Sight (NLOS).

There have been many advances inIPNsystems in the last decade. Bensky outlines the subject of wireless positioning techniques in his book where he describes all the basic principles [3]. He highlights some uses forIPN that have developed in recent years like the many uses in hospitals e.g. tracking the position of doctors and hospital equipment.

The use of IMU for positioning has traditionally been done in marine applications but with the miniaturization of accelerometers and gyroscopes many systems now use IMU:s to track position. Hol published a PhD dissertation on the subject of sensor fusion in 2011 in which he describes the underlying theory of data fusion and how to best implement sensor fusion for positioning [4]. He demonstrated sensor fusion ofIMU with bothUltra Wide Bandwidth (UWB)and vision basedIPNsystems and he emphasizes that

(23)

2 | Introduction

calibration becomes an issue in sensor fusion and describes ways of handling that issue.

The low cost of inertial sensors combined with the prospect of having objects tracking its own movement instead of relying on external antennas or cameras makes inertial navigation desirable. Inertial navigation is due to the large noise in inertial sensors difficult to implement and is currently most often combined with other sensors through sensor fusion. An example of sensor fusion can be read in a paper from Calgary where a Unmanned Aerial Vehicle (UAV) tracked its position through a combination of vision, radar,Global Navigation Satellite Systems (GNSS),IMU, magnetometer and barometer sensor readings [5].

1.1 Background

An emerging area of use forIPNproducts are in factories for quality assurance purposes. Quality assurance is a central issue for many manufacturing industries, e.g. aerospace and automobile, where it is crucial that each fastening is performed correctly to guarantee the safety of the product. Modern factories have central control systems that communicate with machines and tools, trying to prevent errors and costly stops of the factory line. Indoor positional navigation systems can be used to track the position of tools in factories which can be beneficial for quality assurance purposes, assuring that bolts are tightened correctly and can automatically set the tool tightening program for each bolt. Common IPN systems in factories today consist of UWB solutions, which can be decimeter precise, and camera arrays which can reach very high accuracy. Current IPN systems usually involve costly installation and calibration of cameras or antennas, so achieving sufficient accuracy with inertial navigation basedIPNwould be very desirable. For an application where the tool is controlled depending on position it is crucial that theIPN is both accurate and reliable since the operator of the tool needs to quickly and effortlessly be able to change position.

Atlas Copco recently launched two IPN products based on UWB and vision, to track tools in factories. Industrial Location Tethering (ILT) is a radio IPN product based on UWB from Decawave which Atlas Copco use for ranging. The vision based IPNsystem SpotPoint uses two Infrared (IR) cameras and special marker patterns to achieve a high precision on both position and orientation of tools. Common for both theUWBand vision based

(24)

Figure 1.1 – One of Atlas Copco’s electric angle nutrunner tools [1].

IPNis that they are prone to errors inNLOS.UWBsystems are also sensitive to errors from multipath and reflections, especially indoors. A sensor fusion solution where a combination of sensor data is used in the IPNcan help to both increase precision in normal operation and mask erroneous results. Atlas Copco is interested in investigating how an inertial positioning system could function using the IMU:s that are currently in their hand held tools. Atlas Copco have already created an algorithm for inertial navigation and want to test its characteristics and possibilities. Through calibration and characterization of an inertial positioning system its limitations and possibilities can be shown, both as a stand-alone IPN and for possible sensor fusion solutions. In this project a Zero Velocity Update (ZUPT) method for inertial navigation is evaluated for ”bolt to bolt” positioning using theIMUof a modern hand-held tool, see figure1.1. A ”bolt to bolt” scale refers to a positioning accuracy that is acceptable for distinguishing between individual bolts in a fastening pattern.

1.2 Problem

The calculated position from inertial navigation is expected to drift exponentially, because of the integrated noise from the sensors. One way of constricting the IMUnavigation for bolt level positioning is to only sense position changes in- between bolts. AZUPTstrategy for inertial navigation only integrates when an object is moving, which has previously been used for pedestrian tracking [6].

The obvious downside to such a solution is that it will not calculate position in real time, however it can be an efficient way to negate drift. AZUPTinertial navigation algorithm has been developed at Atlas Copco, but its characteristics and potential for bolt level tracking is unknown.

Atlas Copco wants to know the potential of inertial navigation using the

(25)

4 | Introduction

ZUPTstrategy for bolt level positioning using available IMU:s in their tools.

Depending on the findings Atlas would want either:

• To use inertial navigation as stand-alone system.

• To be used together with otherIPNsystems, e.g. covering windows of NLOS or improving accuracy.

The precision and drift of the system would dictate which sort of application the inertial navigation could be applied to:

1. ≈ mm accuracy: measuring bolt-level on close bolts, 2 cm apart;

2. ≈ cm accuracy: measuring bolt-level on far bolts 10-30 cm (e.g. wheel bolts);

3. ≈ dm accuracy: measuring that tool does not leave work area (usually meters in scale);

4. low/no drift: measuring position at all times in workstation;

5. drift: only measuring between bolts.

1.3 Problem formulation

The project aims to study an inertial navigation algorithm developed at Atlas Copco usingZUPTwith linear velocity drift remover. The measurements will be done using a hand held industry tool from Atlas Copco, and performance will be judged from the perspective of positioning for factory quality assurance.

Measurements will be conducted to show the possibilities and best case performance of theIPN, through robot controlled repeatability tests. Empirical measurements on theIMUwill show drift, scale factor and noise which will allow calibration for navigational calculations. The project will judge impact of drift, calibration and sampling speed on the inertial navigation system.

Results will show accuracy of theIPNand the suitable area of usage for quality assurance purposes in hand-held industrial fasteners.

(26)

ZUPT, developed at Atlas Copco. The evaluating study will help identify key aspects, future possibilities and limitations with inertial navigation for bolt level positioning, using current industrial tool hardware.

1.5 Delimitations

The project aims to study inertial navigation for hand held tools for industrial factory lines, where bolt level positioning is the goal. Designing a system which performs the navigational calculations in real time is not part of the project scope. Repeatability measurements will be conducted for the IPN system and data processing will be done separately. The project will only evaluate the IMU currently in the Atlas Copco nutrunner tool. The positioning algorithm will be based on an already implemented version of gravity tracking and Madgwick filter and the project will aim to evaluate its performance.

Similarly the ZUPT and velocity drift remover units will be evaluated and compared with normal integration.

1.6 Ethical perspective

It is unethical for an employer to gather data on their workforce that can be used to gauge the individual worker rather than the task. There have been several scandals of large companies unethically monitoring their workers. Amazon has been hostile towards employees forming unions and is reportedly using a heat map, with more than two dozen metrics, to track which stores are likely to unionize [7] [8]. Sensor data can in some cases be used beyond its main purpose and create unethical breaches of privacy, e.g. researchers have shown that it is possible to judge gender of a person from theIMUdata in smartphones [9]. Even though the data is not used unethically by employers or product owners, the data can still be sensitive which raises questions about security and data storage.

It is important to be truthful with potential shortcomings in industrial positioning technologies considering the applications. Many users of industrial

(27)

6 | Introduction

positioning quality assurance solutions are in automotive or aerospace industries where mistakes can lead to loss of life.

1.7 Structure of the thesis

Chapter 2 presents relevant theory for inertial navigation in general and the specific implementation used in this thesis. Chapter 3 presents the methodology and method used to evaluate the inertial navigation and IMU, as well as explaining test environment and experiments. In chapter4 results are presented with explanations, followed by a discussion of the results in chapter5. In chapter6 conclusions on the ZUPT inertial navigation method for quality assurance are presented as well as comments on future work in the field.

(28)

Chapter 2 Theory

2.1 Position and orientation

Position is synonymous with location and describes where something is.

Position is often defined in coordinates, for example longitude and latitude when describing a position on earth or through x-, y- and z-coordinates in a Cartesian coordinate system. Position is relative to some reference position, which could be earth, a local coordinate system or the last known position.

Changes in position is called translation.

Orientation describes in which way an object is facing and changes in orientation is called rotation. Despite an object being in a set position it can be rotated in any direction. Orientation is commonly described through Euler angles, which defines rotation of an object around its x-, y- and z-axes called roll, pitch and yaw respectively. It is an intuitive way of visualizing rotation, see figure 2.1a. However, Euler angles are prone to singularities which can cause problems in implementation [10].

2.2 Coordinate frames and transformation

Position is relative to a reference position (translation) while orientation is expressed by components of unit vectors of a frame in relation to the reference coordinate frame (rotation). For inertial navigation (of a tool) purposes we can define three relevant coordinate frames:

• Navigation frame (N) or the reference frame of positional coordinates.

(29)

8 | Theory

x y

z

yaw

pitch roll

(a) Cartesian coordinate frame and Euler angles.

IMU

xN

yN

zN

g

xB

yB

zB

(b) Illustration of the IMU body frame B in relation to an arbitrary reference frameN . The direction of gravity can be used to align one axis.

Figure 2.1 – Coordinate frames and rotations.

• Tool frame (T) is the coordinate system based on the tool.

• Body frame (B) is the coordinate frame of the IMU which is rigidly attached to the T frame.

In this project we will use tool frame and body frame interchangeably as they are aligned, however that is not always the case. Figure2.1b illustrates two separate coordinate framesN and B.

2.3 Quaternion representation

Quaternions are a useful way of representing rotations since they do not suffer from the singularity issues that follows from Euler angles [10].

q= w + ix + jy + kz = [w, v] (2.1) where i² = j² = k² = ijk = −1 and ij = −ji = k with real x, y, z

(30)

Quaternion arithmetics

Quaternion arithmetics can be defined as [11]:

q₁+ q₂ = [w1+ w2, v1+ v2] (2.3) q₁− q2 = [w1− w², v1− v²] (2.4) q₁⊗ q2 = [w1w2 − v¹· v2, v1× v2+ w1v2+ w2v1] (2.5)

q₁^∗ = [w1, −v¹] (2.6)

q¹ =w²+ x² + y²+ z² = norm(q1) (2.7) q₁⁻¹ = q₁^∗/norm(q1) (2.8) if norm(q1) = 1, then q₁⁻¹ = q₁^∗ (2.9) A pure quaternion is defined as:

q= [0, v] (2.10)

The multiplication of quaternions is not commutative, as inab = ba, which can be seen from the cross product in equation2.5.

2.3.1 Rotation Quaternion

The quaternion is a four-dimensional vector that can be used to rotate a three- dimensional vector representing the orientation of an object. From equation 2.10we note that a pure quaternion has its real part equal to zero. An arbitrary orientation vector o = [ox, oy, oz] can be expressed as a pure quaternion as q_o = [0, ox, oy, oz]. The orientation o is now expressed in quaternion space as q_o.

To change the orientation of q_o it is multiplied with a quaternion which represents the new orientation. We define a rotation quaternion as qr to be a unit quaternion. To rotate q¯o to the new quaternion q^′_o we multiply with the rotational quaternion and its inverse as [10]:

q^′_o = q_r⊗ qo⊗ qr⁻¹ = q_r⊗ qo⊗ q^∗r (2.11)

(31)

10 | Theory

Since the rotation quaternion is a unit quaternion the length of the orientation quaternion stays constant through rotation.

2.3.2 Converting quaternion representation

Converting axis-angle representation to quaternions can be done through considering a rotation θ about a unit vector ˆn and computing a quaternion as [11] :

q= [w, v] = [cos(1

2θ), ˆnsin(1

2θ)]. (2.12)

The quaternion representation can be converted back to axis-angle representation by:

θ = 2arccos(w), ˆn= v

norm(v) = v

√1 − w² (2.13)

2.3.3 Quaternion rates or angular velocities

The angular velocity ω is related to the rate of change of quaternions [11]. The first time derivative of the unit quaternion is the quaternion rate, and it relates to angular velocities as:

˙q = 1

2q⊗ q^ω∆t (2.14)

where qω = [0, ω]^T and ω = [ωx, ωy, ωz]^T.

2.4 Inertial navigation

An object will resist change in velocity through inertial force. Through Newton’s second law of motion, applying a force of acceleration to an object gives rise to inertia [12]. Inertial navigation is the process of calculating position from inertial forces. If a defines an acceleration vector, v a velocity vector and p a positional vector for a timet, the relationship is described as:

v(t) = v0+ at (2.15)

(32)

rotations and accelerations alone.

2.5 Statistical terms

Common statistical terms helpful in motion detection and positioning.

2.5.1 Accuracy, trueness and precision

International Organization for Standardization (ISO)defines accuracy of a set of measurements to be describing systematic and random errors, meaning that a high accuracy means both high trueness and precision [13]. Trueness refers to how close a measurement is to the true value, which is often measured as the closeness of agreement between the arithmetic mean of measured values and the true value. Accuracy is also commonly used as synonymous with trueness, as purely a description of systematic errors; in this report we use accuracy as in the ISO definition. Trueness is difficult to measure since it requires knowing the true reference value with absolute certainty. In practice trueness is measured against an accepted reference value.

Precision is the closeness of agreement between test results [13]. It describes the random errors of a set of measurements, defined as the inverse of the variance. The precisionp of a variable X with variance V ar(X):

p = V ar(X)⁻¹ (2.17)

Accuracy quantitatively gives a measurement of correctness and the ability to replicate the result which gives credibility to the measurement process.

2.5.2 Standard deviation and Variance

Standard deviation and variance are statistical terms representing the spread of values in a data set. The standard deviation σ is the deviation of a random

(33)

12 | Theory

(a) (b) (c)

Figure 2.2 – Difference between precision, trueness and accuracy: (a) high precision, low trueness (b) high trueness, low precision (c) high accuracy, both high trueness and precision.

variable from its mean, defined as [14]:

σ =

1

N − 1

N

i=1

(xi− ¯x)² (2.18)

The variance is defined as the square of the standard deviation and also as the square of the covariance with the variable itself [14].

V ar(X) = σ² = E[(X − E[X])²] = E[X²] − E[X]² (2.19) An advantage of the standard deviation is that it is of the same unit as the data. Neither the standard deviation or variance are robust to outliers, which mean that they can become arbitrary large by increasing one observation.

2.5.3 Covariance

Covariance is the measure of dependence or joint variability of two random variables. The covarianceCov(X, Y ) between X and Y is defined as [14]:

Cov(X, Y) = E[(X − µ^X)(Y − µ^Y)] (2.20)

Cov(X, Y) = E(XY) − E(X)E(Y) (2.21)

whereµXandµYare the expected values for the stochastic variables X and Y respectively. It follows from equations2.19and2.20that:

Cov(X, X) = V ar(X) (2.22)

(34)

The covariance matrix S is a square matrix containing the covariance between each pair of elements of a set of data [15]. The covariance matrix is symmetrical sinceCov(X, Y) = Cov(Y, X) and the diagonal of the matrix contains the variance, or covariance with the variable itself, which is why the matrix is also called variance-covariance matrix. For a three dimensional set of data in X, Y and Z the covariance matrix is a 3x3 matrix containing the covariances between each variable as:

S_{XY Z} =





V ar(X) Cov(X, Y) Cov(X, Z) Cov(X, Y) V ar(Y) Cov(Y, Z) Cov(X, Z) Cov(Y, Z) V ar(Z)



 (2.24)

2.5.4 Correlation

Another measure of dependency is the correlation. The correlation coefficient betweenX and Y is defined by [14]:

ρ(X, Y) = Cov(X, Y)

σ_Xσ_Y (2.25)

IfCov(X, Y) = 0 then X and Y are said to be uncorrelated. This leads to an important theorem stating that if X and Y are independent they are also uncorrelated. However, the opposite is not true: uncorrelated stochastic variables are not necessarily independent [14].

2.5.5 Principal Component Analysis

Principal Component Analysis (PCA) is an orthogonal decomposition of vectors describing the distribution of data [16]. It is a linear orthogonal transform which makes the transformed data dimensions orthogonal, meaning that they are independent without covariance or correlation. The PCA i calculated from the covariance or correlation matrix through finding a linear transform which makes the data in the matrices diagonal, removing covariances. This is done through calculating eigenvectors and eigenvalues

(35)

14 | Theory

Figure 2.3 – Demonstration of principal component analysis on a set of data.

from the covariance matrix where the orthogonal eigenvectors correspond to the directions in which the data has the largest variance. These vectors are called principal components and form the new coordinate system. The eigenvalue represents the variance along the new axis. The coordinate change inPCAis demonstrated in figure2.3.

2.6 Measurement Characteristics

2.6.1 Noise

Noise is a term for all the unwanted signals in a system. Noise can be caused by a wide variety of things and a common example is the 50 Hz noise often picked up by electronics from the frequency in the power outlets. Random noise is always present to some degree and is usually symbolised as a random variable on the input or output of a system. White noise is defined as any noise which is statistically uncorrelated, or that spikes in noise are independent in time. Therefore the power spectral density of white noise is uniform, meaning equal amplitude over all frequencies [17].

Brown noise is a signal noise produced by a Wiener process, also called random walk, which is a real value stochastic process. Brown noise has a spectral density scaling with1/f²and can be obtained as the integral of white

(36)

Figure 2.4 – Demonstration of noise, offset and scale factor.

noise [17].

W (t) = t

0

dW (τ )

dτ dτ (2.26)

2.6.2 Offset

The offset, also called bias, is a constant displacement of values in a function or a static error in a test set [13]. As a constant it can easily be deducted from a measurement if the offset is known.

2.6.3 Scale factor

The scale factor, or gain, represents the ratio between two functions, e.g.

the input to a sensor to the output signal. This relationship can vary over the measurement range of the sensor and be affected by parameters like temperature. Scale factor, offset and noise are illustrated in figure2.4.

2.6.4 Drift

Drift is when a signal is incrementally changing its value from the expected.

Integration of signals in presence of offset or noise leads to a drift in the calculated value from the expected.

(37)

16 | Theory

2.7 Inertial Measurement Unit (IMU)

The Inertial Measurement Unit IMU is a Microelectromechanical System (MEMS)consisting ofThree Dimensional (3D)accelerometers and gyroscopes.

It is a small and cheap component that is widely used in many technologies like phones, cars or tools. The disadvantages compared to the traditional techniques of measuring accelerations and rotations are reduced accuracy, noise and temperature instability. For positioning purposes the gyroscope and accelerometer data needs to be single and double integrated, which in practise leads to a very short period of valid trajectory prediction due to the non-linearity and noise [18].

The IMU has internal registers that store the accelerometer and gyroscope data which can be read from an external controller. The data is a signed integer corresponding to a number of counts, which can be converted to accelerations and angular velocities through conversion tables of the IMU data-sheet. The rate at which these registers are updated is commonly called Original Data Rate (ODR)which is synonymous to the sample rate or bandwidth of the IMU.

Sampling the data from the IMU in a faster rate than theODRis redundant.

2.7.1 Accelerometers

An accelerometer is a device measuring acceleration along a single axis of movement. This is traditionally implemented through suspending a weight between springs along the axis and measuring acceleration by the displacement of the weight from the equilibrium. MEMSaccelerometers also operate by sensing the displacement of a weight but use varying techniques to do it.

It is common to use a flexing beam of a known weight, measurements of displacement can be done e.g. by measuring strain, using optics like fiber Bragg grating or by measuring frequency shifts in resonance of the beams [19][20][21].

2.7.2 Gyroscopes

Gyroscopes measure rotational velocity around an axis. Traditional methods like the ring laser gyroscope have been widely used in inertial navigation proposes but are too expensive for low cost applications. There are several ways of implementingMEMSgyroscopes, e.g. vibratory gyroscopes making use of the Coriolis effect [22]. These gyroscopes have a vibrating structure

(38)

2.8 IMU calibration methods

2.8.1 6-point tumble calibration

The 6-point tumble calibration is a method for calculating the bias, gains and cross-axis gains of three axis accelerometers. It is named after the method of positioning the3Daccelerometer in six distinct positions, according to figure 2.5. In each configuration one of the three axes are pointing either towards or against the gravitational force while the other two axes are kept parallel to the ground. The axes aligned with the gravitation should experience accelerations of±1 g while the other axis accelerations should be zero.

The calculation of scale factor for each axis is done by considering the output of the two positions where the axis is aligned towards and against the gravitational force [23]. LetAidenote the accelerometer parameter for axisi, and scale factor is calculated through:

Ai,scalef actor = (Ci,g − C^i,−g)/(2Cexpected) (2.27) Where Cg and C−g are the average counts for the IMU axis with g in positive direction and negative respectively, Cexpected is the expected amount of counts for the axis withg acceleration. The offset is calculated by:

Ai,of f set= (Ci,g + Ci,−g)/2 (2.28) where the offset A is given in counts.

2.8.2 Gyroscope calibration

Calibration of a gyroscope can be done through rotating the IMU in a known angular velocity. This is tricky in practise and an easier way is to rotate the IMU along one rotational axis a set angular displacement, where the angle displacement can be calculated from the gyroscope data through integration.

(39)

18 | Theory

z

x y

(a)

z y

(b)

z

x

y

(c)

z

x

(d)

y z

(e)

x

y

(f) Figure 2.5 – Configurations of the 6-point tumble calibration.

A correction of the scale factor can then be calculated through:

correction = θM/θ (2.29)

Where θ_M and θ are the measured and the actual angular displacement respectively. The offset of each gyroscope can be found from calculating an average from the output of the gyroscope when the IMUis being kept still.

A practical approach of performing such a calibration is proposed by Analog Devices [24].

The offset of a gyroscope can vary with orientation due to gravitation. To find a mean of the offset we can study the gyroscope output for each position of the 6-point tumble calibration.

2.9 Error sources in inertial navigation

2.9.1 IMU errors

TheIMUsuffers from noise, offset and scale factor errors for both accelerometers and gyroscopes. It is also sensitive to temperature and these errors can change in a non-linear way [18]. TheIMU commonly has cross-gains between the axes as a result of imperfect orthogonal alignment. The integration and double

(40)

AnIMUmovement in the reference frameN will be calculated as a movement with arbitrary alignment in the IMUcoordinate frameB. Errors in aligning theB to N coordinate frame will impact the accuracy of the system. Drift in calculated orientation and position for longer movements further emphasize the issue.

2.10 Common positional calculation methods

2.10.1 Kalman filter

The Kalman filter is an efficient recursive filter algorithm that uses partial or noisy measurements to predict the state of a dynamical system and corrects the state based on new measurements and prior estimates. The method is widely used in many different technological areas, from aerospace to video-games, and has been the focus of extensive research [25].

The Kalman filter is a recursive algorithm where the state xkis considered a Gaussian random variable. The filter is generally described in two steps:

prediction and correction.

For a linear system in discrete time we can consider a system estimating the statex, governed by:

xk = Ak−1xk−1+ Bk−1uk−1+ wk−1 (2.30)

yk= Hkxk+ vk (2.31)

wherewkrepresents process noise andvkrepresents a measurement noise.

Then × n matrix A relates the state at step k to the state at k + 1, when there is no driving function or or process noise, and then × l matrix B relates the input uk to state x. Both wk and vk are white noise with an average value of zero and variance Ωk and Σk respectively at time k [25]. The Kalman filter estimates the state x^k from earlier measurements ofuk−1, yk−1 and the

(41)

20 | Theory

previously predictedxk−1.

The a priori estimate xˆ⁻_k at stepk is calculated using knowledge of the process before step k. ˆxk is the a posteriori estimate after knowledge of yk

measurement. The Kalman filter predicts future measurement value Hkxk

and the discrepancy between the prediction and the actual measured value yk is called innovation or residual, ν = (zk − H^kxˆ⁻_k). The Kalman filter computes an a posteriori as a linear combination of the innovation and the a prioriestimatexˆ⁻_k:

ˆ

xk= ˆx⁻_k + K(zk− H^kxˆ⁻_k) (2.32) The Kalman gain K is a weighting between the predicted measurement ˆ

x⁻_k, from the process, and the measured valueyk.

2.10.2 Extended Kalman filter

Previous section assumes linear stochastic differential equation governing the relation between observation, system state and additive noise. If the process or the relationship between measurement and process are non-linear, additional methods needs to be used. TheExtended Kalman Filter (EKF)linearizes about the mean and covariance to deal with such situations [25].

It is possible to apply theEKFby making use of the Taylor series expansion of the nonlinear elements about the current estimates. Let Jf and Jg denote the Jacobian matrix of the function f and g respectively. The first order estimate aboutk − 1 yields [11]:

xk+1 = gk(ˆxk−1) + Jg(ˆxk−1)(xk− ˆx^k+1) + wk (2.33)

yk= fk(ˆxk−1) + Jf(ˆxk−1)(xk− ˆx^k−1) + vk. (2.34) Rearranging yields a linear form appropriate for the previously defined Kalman filter:

˜

xk+1 = xk+1− g^k(ˆxk−1) + Jgkxˆk−1 = Jgkxk+ wk (2.35)

˜

y = yk− f^k(ˆxk−1) + Jf kxˆk−1 = Jf kxk+ vk (2.36) wherex and ˜˜ y are synthetic state and observation variables.

(42)

sensorS relative to the earth frame E from two orientation calculations. The first calculation is orientation ^SEqω,t derived by integration of angular rates

Sω and the second derives orientation ^S_Eq_∇_,t from the influence of direction of gravity on the accelerometers. A grade descent algorithm iterates to find a quaternion to describe the orientation of the accelerometers with respect to gravity, and the same approach can be applied for magnetometers for tracking orientation based on the magnetic field. When fusing the orientation calculations the algorithm uses the divergence rate β of ^S_Eqω, which is the magnitude of a quaternion derivative expressing the gyroscope error, to ensure that the weighted divergence is equal to the weighted convergence of^S_Eq_∇[26].

2.11 Positioning algorithm

Figure 2.6 – Block diagram of positioning algorithm developed at Atlas Copco.

There are two main blocks: the filter module (upper, light grey) and the integration module (lower, light yellow). The filter module calculates the tilt compensated dynamic accelerations, which can be integrated directly into position. The integration module appliesZUPT, motion detection and velocity drift remover, before integrating into the resulting position.

The structure of the algorithm developed at Atlas Copco can be seen in figure 2.6. The algorithm first uses a gravity tracking Kalman filter and removes the gravity acceleration from the raw data of the accelerometers, which results in the dynamical accelerations. A Madgwick filter is used to

(43)

22 | Theory

track the change in orientation during a measurement. The orientation tracking and dynamical accelerations are then combined to find the tilt compensated dynamical accelerations. These are the accelerations expressed in x, y and z of the original orientation of the IMU with the start of motion in origo.

This coordinate frame will be referenced as the positioning frame. The tilt compensated dynamical accelerations can be directly integrated twice into position, however this results in inaccurate drifting results. In this algorithm the ZUPT method is used by motion detection and a linear velocity drift removal.

2.11.1 Gravity sensing Kalman filter

The gravitational force g is an acceleration which has to be accounted for in inertial navigation. Fundamental to inertial navigation is to integrate accelerations that lead to a change in position or orientation. As described above it is important to remove any offset in accelerometer or gyroscope value or the integration will lead to an exponentially growing error. The magnitude of the offset from gravity is known but its direction in reference to the IMU is not, creating a classical tracking problem. It is not trivial to choose a fitting state-space model for gravitational tracking and a quaternion based representation is not always the best [27]. In the inertial navigation algorithm evaluated in this thesis we use the adaptive Kalman filtering and smoothing created by S. Särkkä, V. Tolvanen, J. Kannala, and E. Rahtu, published 2015 [27]. The main concepts of that gravity tracking filter is explained below.

Suppose that the IMU is placed within the tool and aligned so that the IMU and tool coordinate axis align to the tool-local coordinate system with subscriptT . The measurement from the accelerometers will return both the dynamical accelerations and the gravitational acceleration as:

a_{IM U} = g_T(t) + aT(t) (2.37) We are interested in tracking the gravitational acceleration g_T that we assume to be on average dominant over the dynamical accelerations aT. Therefore we can model aT as a random white noise ǫ(t) on the measurements of g_T from the accelerometers.

aIM U = g_T(t) + ǫ(t) (2.38)

The gravitation g_T is not constant, however the gravity relative earth g_N

(44)

Quaternion free representation for gravitational tracking

Quaternion representation is not observable without independent orientation measurement which is not available when only working with an IMU without, for example a measurement of the magnetic field. Another issue that appears when working with quaternions is that the non-linear Kalman filter can run into numerical problems due to the requirement of unit length. Process noise and estimation errors distort the lengths of quaternions from unity causing eigenvalues to be zero. Using stochastic gradient descent, as the Madgwick filter described in section2.10.3, is a way to reformulate the problem but this leaves the covariance of the unobservable subspaces to grow unbounded until the filter eventually diverge [27]. A quaternion free representation is derived below.

The equation for the tool-local gravitation is given as:

g_T(t) = q^∗(t)g_Nq(t) (2.40) Since g_N is a constant global acceleration we can differentiate to:

d

dtg_T(t) = d

dt[q^∗(t)g_Nq(t)] = −ω^T(t) × g_T (2.41) If we then add additional noise process into the equation and replace ω_T(t) with noisy gyroscope measurements ωIM U(t), we arrive at following quaternion free equation:

d

dtg_T(t) = −ω^{IM U}(t) × g_T(t) + wg(t) (2.42) A non-linear Kalman filter is applied to the state space model to find the local gravitation vectors g_T(t).

Adaptive gating

Accelerations measured by the IMU can not always be treated as some small noise, as they can be large compared to gravity in some cases. When a high acceleration is applied the calculated gravitation direction leans towards the

(45)

24 | Theory

acceleration. To diminish this effect an adaptive gating is used, which during periods of high acceleration increases the measurement noise of the model.

At each measurement a test is made for the innovation ν, and innovation covariance S_k[27]:

ν^T

kS⁻¹_k ν_k > γ (2.43)

where γ is a fixed threshold. If the expression in equation 2.43 is true a measurement noise peak α is inserted with a decaying time behaviour of

d

dtα = −(1/τ). The gravity tracking filter in this thesis is using α = 100, τ = 1/2, γ = 1/100 and w = 5 × 10⁻⁵ for calculations made in 400 Hz.

Altering frequency means that the parameters should be adjusted for the lower interval between samples.

2.11.2 ZUPT

” Zero Velocity Update”ZUPTis a strategy mainly developed for pedestrian positional navigation utilizing IMU:s in the pedestrian’s shoes. ZUPTuses the zero velocity condition in each step of a pedestrian to calculate navigational errors and update position rather than to continuously integrate the output of the IMU [6]. Since the positional error grows exponentially with time it is beneficial to control the duration of free navigation. This method can reduce the drift in position from an inertial navigation system given that the IMU is frequently stopping. To implementZUPTthere needs to be a strategy for motion detection, which aims to differentiate noise from movement. One strategy is to set thresholds on accelerations and angular velocities. After detection of the start and stop of movement, the zero velocity condition can be used to estimate the drift to reduce calculation errors.

2.11.3 Velocity drift remover

The calculated velocity will be affected by an integration drift from the noise of theIMU. A strategy to remove the linear components of drift is to use the start and end points of a detected movement and estimate a linear error in the velocity, demonstrated in figure 2.7. This error is then removed before integrating the velocity to position.

(46)

Figure 2.7 – Principle of velocity drift remover. The algorithm fits a linear curve on calculated velocity between the detected start of movement and end of movement and subtracts it from the velocity.

2.11.4 Position coordinate frame

The tilt compensated dynamical accelerations that are integrated into position are expressed in terms of positioning coordinates which will start in origo for the first motion where the original orientation of the tool coordinate frame and the positioning coordinate frame are aligned. This reference frame is what is being used to judge measurements, meaning that the initial acceleration of each measurement defines the reference frame.

(47)

26 | Theory

(48)

Chapter 3 Method or Methods

3.1 Test environment

3.1.1 Industrial nutrunner

The tool used in this project is a battery driven electric angle nutrunner tool from Atlas Copco. The main board of the tool is located in the handle and base of the tool and contains two separate Micro-Controller Unit (MCU):s which controls the tool and communicate with external factory controller. The IMU is located near the base of the tool.

The MCU:s run at 8 kHz but due to power saving, the MCU:s lower their clock frequency when the tool is not running a tightening. The frequency floats between 7 to 8 kHz, which also affects the data transfer frequency used in measurements.

3.1.2 ISM330DLC IMU

The IMU used on the Atlas Copco nutrunner tools is the ISM330DLC from ST Electronics. It is a MEMS System-in-Package (SiP)containing3D accelerometer and gyroscope as well as an embedded thermometer [2].

The IMU can be configured for sampling rate, full scale settings and filtering. The IMU is configured for high performance with its maximumODR of 6.66 kHz. The full scale setting determines the sensitivity of the sensors by adjusting the maximum acceleration value of the output signal. TheIMUcan be adjusted for a full-scale acceleration range of±2/ ± 4/ ± 8/ ± 16 g and

(49)

28 | Method or Methods

Figure 3.1 – Measurement and configuration set-up for the tool.

angular rate of±125/±250/±500/±1000/±2000degrees per second (dps).

3.1.3 Data communication

The data communication for the measurements are shown as a block diagram in figure3.1.

The toolMCUis communicating at 8 kHz through a UART cable which has six communication slots for debugging. The MCU can send six 16-bit values at 8 kHz, which are used for the IMU data from3Daccelerometers and gyroscopes.

The data is transferred through an adapter PCB and a generic target cable to an Atmel SAMv71 Xplained Ultra MCU. The Atmel MCU is used to transfer the data into two CAN bus cables which are connected to theData Acquisition Unit (DAQ). The DEWE-CAN2 is a DAQwhich records the data from the CAN busses and transfers the data to a PC through USB.

A J-link debugger is used to configureMCU on the tool, connecting the coding environment on the PC to the tool.

(50)

Figure 3.2 – The UR5 industrial robot with the nutrunner tool mounted. The robot interface is shown in the bottom of the image.

3.1.4 UR5 Industrial Robot

The UR5 from Universal Robots is an industrial robot developed to be used working with humans in close proximity, a s.c. Cobot. The UR5 is used in this study as ground truth reference in movements for all measurements. In figure 3.2the UR5 with the mounted nutrunner tool is shown.

The UR5 has a pose repeatability perISO9283 of ±0.03 mm [28]. The resolution in rotation is 0.001 radians, which enables high accuracy translation and rotation. ISO 9283 is a standard for performance criteria and related test methods of industrial robots which deals with testing specific parameters like pose-to-pose and path characteristics [29].

3.2 IMU setting and calibration measurements

3.2.1 Full scale setting test

In order to get the maximum resolution of accelerations the sensitivity of the sensors should be set as high as possible without saturation. To test the scale of accelerations that can be expected from theIMUthe tool is handled by hand with quick violent movements and rotations for all axes of the IMU. Each

(51)

sensitivity setting is investigated where the output of these violent movements are seen as a maximum output for regular use.

3.2.2 Low pass filter setting

The built in low-pass filters of the IMU are used to filter out noise and it is important to set the cutoff frequency appropriately so that ideally all noise is filtered out without loosing signals of interest. Since accelerations from hand held movements are of a low frequency, and noise from electronics and tool engine will be present a low-pass filter will be used. TheIMUon board filter and its four settings for both accelerometer and gyroscope will be tested. The settings available when using anODRof 6.66 kHz are shown in table3.1.

Setting Accelerometer Gyroscope

fc0 16.7 Hz 173 Hz

fc1 67 Hz 237 Hz

fc2 133 Hz 351 Hz

fc3 740 Hz 937 Hz

Table 3.1 – Low-pass filter cut-off frequencies

Each setting is tested by rotating and shaking the tool, hitting it against a stable object (pushing against a nut) and letting the motor within the tool accelerate, in air, from standstill up to the maximum 1205 rpm and rotating for around 5 seconds. The IMU settings for full scale output used are 4 g and 500dps. The chosen settings for this evaluation is to use the lowest available cut-off frequency for both gyroscopes and accelerometers.

3.2.3 Calibration measurements

For both accelerometer and gyroscope calibration, the tool is rigidly fastened to the UR5 robot which controls rotation and movement of the tool. The alignment is done using the tool central axis which is in alignment of the IMU x-axis.

The six point tumble calibration is performed according to section2.8.1.

The tool is initially leveled by hanging a long weighted thread next to the tool and aligning the long tool x-axis with the thread. Measurement of the accelerometers outputs are taken over 5 seconds. The robot controls 90 degree movements to place the tool in each of the six positions.

(52)

the gyroscope does not get any gravitational offset.

3.3 Positioning measurements

Positional measurements are carried out using the test set-up described above.

Both handheld tests and precise robot controlled tests are studied. When judging the accuracy of the system it is important consider that the calculated positional changes are from the perspective of the IMU which is located at the bottom of the tool. For the robot controlled measurements an offset of the Tool Center Point (TCP)is set to the position of the IMU so that the translation and rotation of the robot is centered on the IMU (position of theTCP). The manual measurement of theTCPis tested through conducting measurements where the tool is rotated and the output is studied. Changes of mm scale in position was made to find the minimum output of acceleration for rotations around theTCP. An estimated maximum error of position of theTCPto the actual position of the IMU is±5mm.

3.3.1 30 cm linear movement

This experiment is conducted to be able to show how mean accuracy and precision varies with degree of rotation and show differences due to the direction of gravity with respect to the motion.

Experiment

Linear movement of 30 cm with varying degree of rotation, testing both movement parallel- and orthogonal to gravity. Rotations were divided into 0^◦, 30^◦, 60^◦ and 90^◦ degree rotation in pitch, yaw and roll separately. The measurement is carried out by the robot performing the movement with smooth rotation, stops at 30 cm, and then makes the inverted motion to end up at the original position. Graphs presented only show results from the motion in positivex direction.

(53)

3.3.2 Movement in square shape

Experiment

The measurement series is robot controlled and conducted through moving the IMU in a square shape. A number of factors are varied in the measurement to study their impact. These factors are explained below.

Size of square

The impact of changing size of the motion is studied through making measurements of movements with both a 20 cm and 40 cm side length. A measurement series without rotation is also conducted for a square of small side length of 2 cm.

Frequency of stops

To study the impact of using motion detection to reduce drift at each stop, each motion is stopped at different intervals. For each square motion three sets of measurements are conducted:

1. continuous motion, only stopping at the end;

2. stop twice per revolution;

3. stop at each corner.

Every intermediate stop is 1 s long.

Degree of rotation

The impact of rotations on the square shape is studied by adding one and several axes of rotation. The rotation is set to be 45^◦ per side of the square, moving to 90^◦ rotation in the second corner and then changes rotational direction to end up at the original position at the end of the motion.

Gravitational direction

The measurements of the 20 cm side length square is performed both in a plane parallel to gravity and in a plane orthogonal to gravity. The other measurements are conducted only in a plane orthogonal to gravity.

(54)

accuracy robot movements. Repeatability tests enables judging the overall accuracy of the system.

Truthfulness is judged by looking at the difference in the mean of the final position or known intermediate coordinates (like stops in movement) of a set of measurements and the expected position.

Precision will be judged usingPCAof position data for each measurement.

To be able to easily compare measurements with varying distributions a precision index of the standard deviation|σ| is used. It is calculated by finding the absolute length of the standard deviation σ along all orthogonal PCA vectors.

|σ| =(σ1)²+ (σ2)²+ (σ3)² (3.1) where σ_i is the square root of the eigenvalue λ_i for eigenvectoreˆi of the covariance matrix of the position data.

3.4.2 Velocity drift remover

Impact of the velocity drift remover can be studied by comparing the calculated position with results from integrating the tilt compensated dynamical accelerations directly.

3.4.3 Parameters of Gravity Tracking filter

Two main parameters in the gravity tracking filter are adjusted: the innovation outlier threshold γ and the values of noise in process and measurement. The innovation corresponds to the residual of the minimization in the Kalman filter and the outlier threshold γ dictates how large variations in acceleration that the filter will accept when tracking gravity. This value should be suited to the magnitude of accelerations and how rapidly rotations and accelerations are expected to change. The suitable outlier threshold γ will also change

(55)

depending on sample rate and if averaging between samples is used. The expected jump in gravity is lower when sampling more frequently or if several samples are averaged. Unless stated otherwise the results are calculated with a sample frequency of 400 Hz without averaging, where a γ = 1/100 is used.

The measurement noise of the Kalman filter corresponds to the noise of the accelerometers and gyroscopes which is empirically measured. The process noise is a measure of how much we trust the accuracy of the model. The model is extremely accurate as a natural law but a suitable number for the weighing in the filter is acquired through testing (w^k = 0, 00005 in this case).

3.4.4 Parameters of Motion Detection algorithm

The implementation of the motion detection is different from theZUPTused for pedestrian tracking although the concept is the same [6]. Using ZUPT when the IMU is in a shoe is helped by the sharp peak when the foot hits the ground that can be used to detect the stop of motion; that technique cannot be used for a hand-held device. The algorithm is instead designed as to study three parameters to detect motion:

• |a| Magnitude of acceleration,

• |ω| Magnitude of angular velocity,

• Cov(ax, ay, az) Covariance of accelerations between the three axes.

Each parameter is measured against a threshold and motion is detected as soon as any of the three parameters crosses the threshold. A plot of how a detection curve can look like is shown in figure3.3.

The motion detection parameters will be specifically adjusted for each measurement so that the motion detection accurately detects the true start and end of movement.

3.4.5 Impact of sample frequency and averaging

Due to power saving and constraints in hardware it is unlikely to run this algorithm for a full 6.66 kHz sample speed in a future implementation. Hence it is important to study the impact of sample frequency on the accuracy of the IPN. Another consideration when limiting the sample frequency is whether to

Calibration and Evaluation of Inertial Navigation Using Zero Velocity Update for Industrial Tools

Calibration and Evaluation of Inertial Navigation Using Zero

Velocity Update for Industrial Tools

JOHAN RÅGMARK

JOHAN RÅGMARK

Keywords

Nyckelord

Contents

List of Figures

List of Tables

Chapter 1 Introduction

1.1 Background

1.2 Problem

1.3 Problem formulation

1.5 Delimitations

1.6 Ethical perspective

1.7 Structure of the thesis

Chapter 2 Theory

2.1 Position and orientation

2.2 Coordinate frames and transformation

x y

z

2.3 Quaternion representation

2.3.1 Rotation Quaternion

2.3.2 Converting quaternion representation

2.3.3 Quaternion rates or angular velocities

2.4 Inertial navigation

2.5 Statistical terms

2.5.1 Accuracy, trueness and precision

2.5.2 Standard deviation and Variance

2.5.3 Covariance

2.5.4 Correlation

2.5.5 Principal Component Analysis

2.6 Measurement Characteristics

2.6.1 Noise

2.6.2 Offset

2.6.3 Scale factor

2.6.4 Drift

2.7 Inertial Measurement Unit (IMU)

2.7.1 Accelerometers

2.7.2 Gyroscopes

2.8 IMU calibration methods

2.8.1 6-point tumble calibration

2.8.2 Gyroscope calibration

2.9 Error sources in inertial navigation

2.9.1 IMU errors

2.10 Common positional calculation methods

2.10.1 Kalman filter

2.10.2 Extended Kalman filter

2.11 Positioning algorithm

2.11.1 Gravity sensing Kalman filter

2.11.2 ZUPT

2.11.3 Velocity drift remover

2.11.4 Position coordinate frame

Chapter 3

Method or Methods

3.1 Test environment

3.1.1 Industrial nutrunner

3.1.2 ISM330DLC IMU

3.1.3 Data communication

3.1.4 UR5 Industrial Robot

3.2 IMU setting and calibration measurements

3.2.1 Full scale setting test

3.2.2 Low pass filter setting

3.2.3 Calibration measurements

3.3 Positioning measurements

3.3.1 30 cm linear movement

3.3.2 Movement in square shape

3.4.2 Velocity drift remover

3.4.3 Parameters of Gravity Tracking filter

3.4.4 Parameters of Motion Detection algorithm

3.4.5 Impact of sample frequency and averaging