Won_Peter - UWSpace Home

Intelligent Fastening Tool Tracking
Systems Using Hybrid Remote Sensing
Technologies
by
Peter S. Won
A thesis
presented to the University of Waterloo
in fulfillment of the
thesis requirement for the degree of
Doctor of Philosophy
in
Mechanical Engineering
Waterloo, Ontario, Canada, 2010
© Peter S. Won 2010
Author’s Declaration
I hereby declare that I am the sole author of this thesis. This is a true copy of the thesis, including any
required final revisions, as accepted by my examiners.
I understand that my thesis may be made electronically available to the public.
ii
Abstract
This research focuses on the development of intelligent fastening tool tracking systems for the
automotive industry to identify the fastened bolts. In order to accomplish such a task, the position of
the tool tip must be identified because the tool tip position coincides with the head of the fastened bolt
while the tool fastens the bolt. The proposed systems utilize an inertial measurement unit (IMU) and
another sensor to track the position and orientation of the tool tip.
To minimize the position and orientation calculation error, an IMU needs to be calibrated as
accurately as possible. This research presents a novel triaxial accelerometer calibration technique that
offers a high accuracy. The simulation and experimental results of the accelerometer calibration are
presented.
To identify the fastening action, an expert system is developed based on the sensor measurements.
When a fastening action is identified, the system identifies the fastened bolt by using an expert
system based on the position and orientation of the tool tip and the position and orientation of the bolt.
Since each fastening procedure needs different accuracies and requirements, three different systems
are proposed.
The first system utilizes a triaxial magnetometer and an IMU to identify the fastened bolt. This
system calculates the position and orientation by using an IMU. An expert system is used to identify
the initial position, stationary state, and the fastened bolt. When the tool fastens a bolt, the proposed
expert system detects the fastening action by triaxial accelerometer and triaxial magnetometer
measurements. When the fastening action is detected, the system corrects the velocity and position
error using zero velocity update (ZUPT). By using the corrected tool tip position and orientation, the
system can identify the fastened bolts. Then, with the fastened bolt position, the position of the IMU
is corrected. When the tool is stationary, the system corrects linear velocity error and reduces the
position error. The experimental results demonstrate that the proposed system can identify fastened
bolts if the angles of the bolts are different or the bolts are not closely placed. This low cost system
does not require a line of sight, but has limited position accuracy.
The second system utilizes an intelligent system that incorporates Kalman filters (KFs) and a
fuzzy expert system to track the tip of a fastening tool and to identify the fastened bolt. This system
employs one IMU and one encoder-based position sensor to determine the orientation and the centre
of mass location of the tool. When the KF is used, the orientation error increases over time due to the
iii
integration step. Therefore, a fuzzy expert system is developed to correct the tilt angle error and
orientation error. When the tool fastens a bolt, the system identifies the fastened bolt by applying the
fuzzy expert system. When the fastened bolt is identified, the 3D orientation error of the tool is
corrected by using the location and the orientation of the fastened bolt and the position sensor
outputs. This orientation correction method results in improved reliability in determining the tool tip
location. The fastening tool tracking system was experimentally tested in a lab environment, and the
results indicate that such a system can successfully identify the fastened bolts. This system not only
has a low computational cost but also provides good position and orientation accuracy. The system
can be used for most applications because it provides a high accuracy.
The third system presents a novel position/orientation tracking methodology by hybridizing one
position sensor and one factory calibrated IMU with the combination of a particle filter (PF) and a KF.
In addition, an expert system is used to correct the angular velocity measurement errors. The
experimental results indicate that the orientation errors of this method are significantly reduced
compared to the orientation errors obtained from an EKF approach. The improved orientation
estimation using the proposed method leads to a better position estimation accuracy. The
experimental results of this system show that the orientation of the proposed method converges to the
correct orientation even when the initial orientation is completely unknown. This new method was
applied to the fastening tool tracking system. This system provides good orientation accuracy even
when the gyroscopes (gyros hereafter) include a small error. In addition, since the orientation error of
this system does not grow over time, the tool tip position drift is limited. This system can be applied
to the applications where the bolts are closely placed. The position error comparison results of the
second system and the third system are presented in this thesis. The comparison results indicate that
the position accuracy of the third system is better than that of the second system because the
orientation error does not increase over time.
The advantages and limitations of all three systems are compared in this thesis. In addition,
possible future work on fastening tool tracking system is described as well as applications that can be
expanded by using the KF/PF combination method.
iv
Acknowledgements
First of all, I cannot thank God enough for his step by step guidance. Without his help, I could
not even have begun studying. Also, I am very grateful that he sent me many people to help and
support me.
Among those people, I would like to express my gratitude to my two supervisors, Prof. William
Melek and Prof. Farid Golnaraghi. With their continuous support, guidance, and advice, not only I
have improved my research ability, but also I have achieved great improvement on my personality.
I would like to thank all defence committee members, Prof. Glenn Helppler, Prof. Robert Gorbet,
Prof. Jan Huissoon, and the external examiner Prof. Guangjun Liu. They read my thesis very
carefully and gave me good advice to make my thesis stronger. I also want to thank my colleagues,
especially Dr. Orang Vahid and Mr. Ying Kun Peng, for the help, support, and friendship.
Last, but not least, I really want to express my appreciation to my parents, my parents-in-law, and
most of all, my wife, I-yung for their prayer, love, and endless support.
v
Table of Contents
Author’s Declaration .............................................................................................................................. ii
Abstract ................................................................................................................................................. iii
Acknowledgements ................................................................................................................................ v
Table of Contents .................................................................................................................................. vi
List of Figures ....................................................................................................................................... ix
List of Tables ....................................................................................................................................... xiii
Chapter 1 Introduction............................................................................................................................ 1
1.1 Motivation .................................................................................................................................... 1
1.2 Literature Review ......................................................................................................................... 2
1.2.1 IMU Calibration .................................................................................................................... 2
1.2.2 Position and Orientation Computation Using an IMU .......................................................... 4
1.2.3 Position Sensors..................................................................................................................... 6
1.2.4 Orientation Detection Using Position Sensors ...................................................................... 8
1.3 Thesis Overview ........................................................................................................................... 8
1.4 Contributions ................................................................................................................................ 9
Chapter 2 Theoretical Preliminaries ..................................................................................................... 10
2.1 Hybridization Techniques .......................................................................................................... 10
2.1.1 Bayes Filter.......................................................................................................................... 10
2.1.2 Kalman Filter [91] ............................................................................................................... 11
2.1.3 Sampling Importance Resampling Particle Filter [93] ........................................................ 16
2.2 Fuzzy Expert Systems ................................................................................................................ 18
2.2.1 Fuzzy Set ............................................................................................................................. 19
2.2.2 Fuzzy Logic ......................................................................................................................... 20
Chapter 3 Accelerometer Calibration Technique ................................................................................. 23
3.1 Overview .................................................................................................................................... 23
3.2 Iteration Method to Calculate Axes Gains and Biases ............................................................... 25
3.3 Numerical Analysis .................................................................................................................... 28
3.4 Experiments ................................................................................................................................ 31
3.4.1 Experiments Using a Triaxial Accelerometer with Three Identical Single-Axis
Accelerometers ............................................................................................................................. 32
vi
3.4.2 Experiments Using a Triaxial Accelerometer with Different Gains ................................... 38
3.5 Conclusion .................................................................................................................................. 42
Chapter 4 Fastened Bolt Tracking System Using an IMU and a Triaxial Magnetometer with an Expert
System .................................................................................................................................................. 44
4.1 Proposed Tracking System ......................................................................................................... 44
4.2 Orientation Representations ....................................................................................................... 45
4.2.1 Direction Cosine Matrix ...................................................................................................... 45
4.2.2 Quaternion ........................................................................................................................... 46
4.3 Position Estimation..................................................................................................................... 47
4.4 Expert System............................................................................................................................. 49
4.4.1 Stationary State Identification ............................................................................................. 49
4.4.2 Initial Position Detection ..................................................................................................... 52
4.4.3 Fastening Action Detection ................................................................................................. 54
4.5 Experiments ................................................................................................................................ 59
4.6 Conclusion .................................................................................................................................. 63
Chapter 5 Fastening Tool Tracking System Using an IMU and a Position Sensor with Kalman Filters
and a Fuzzy Expert System .................................................................................................................. 64
5.1 Position Sensor Selection for Tool Tracking System ................................................................. 64
5.2 Tool Tracking System Design with an IMU and an Encoder-Position Sensor .......................... 65
5.2.1 Orientation Calculation Using Kalman Filtering................................................................. 67
5.2.2 Position Kalman Filter Using an IMU and a Position Sensor ............................................. 69
5.3 Fuzzy Expert System for Tool Tracking System........................................................................ 71
5.3.1 Tilt Angle Correction .......................................................................................................... 71
5.3.2 Fastening Action Detection ................................................................................................. 72
5.3.3 Fastened Bolt Identification ................................................................................................ 74
5.3.4 Orientation Correction ......................................................................................................... 77
5.4 Experiments ................................................................................................................................ 79
5.5 Conclusion .................................................................................................................................. 81
Chapter 6 Fastening Tool Tracking System Using a Combined Kalman/Particle Filter ...................... 83
6.1 Overview of the Position/Orientation Tracking System Combining the KF and the PF............ 83
6.2 Position Kalman Filter................................................................................................................ 85
vii
6.3 Orientation Filtering Technique ................................................................................................. 86
6.3.1 Angular Velocity Correction Using an Expert System........................................................ 88
6.3.2 Initial Orientation Estimation .............................................................................................. 91
6.4 Experiments ................................................................................................................................ 92
6.4.1 Preliminary Experiment....................................................................................................... 92
6.5 PF-KF-Based Tool Tracking System ....................................................................................... 104
6.5.1 Application to Tool Tracking System - Theory................................................................. 104
6.5.2 Experiment Results............................................................................................................ 107
6.6 Conclusion ................................................................................................................................ 116
Chapter 7 Thesis Contributions and Future Work .............................................................................. 118
7.1 Thesis Contributions................................................................................................................. 118
7.2 Future Work ............................................................................................................................. 119
Appendix A : Quaternions .................................................................................................................. 121
Appendix B : Gyro Calibration .......................................................................................................... 126
Bibliography ....................................................................................................................................... 128
viii
List of Figures
Figure 1-1: Position error simulations of different grades of IMUs [11] ............................................... 5
Figure 2-1: Fuzzification example........................................................................................................ 20
Figure 2-2: Inference mechanism. ........................................................................................................ 22
Figure 3-1: Flow chart diagram of the proposed triaxial accelerometer calibration method. .............. 24
Figure 3-2: Calibration procedure: the proposed calibration method requires six different tilt angle
measurements to determine six calibration parameters. ............................................................... 24
Figure 3-3: Procedure for the simulation calibration and parameter validation. .................................. 29
Figure 3-4: Tilt angle contents of the simulations: tilt angles for (a) Simulation 1, (b) Simulation 2, (c)
Simulation 3, (d) Simulation 4, and (e) Simulation 5. .................................................................. 30
Figure 3-5: Experimental system setup. ............................................................................................... 31
Figure 3-6: Procedure for the calibration experiment and parameter validation. ................................. 32
Figure 3-7: Colibrys triaxial accelerometer on a 3-way milling vise: the sensor axes (XY) and the
fixed frame axes (xy). ................................................................................................................... 33
Figure 3-8: Rotation sequence of a triaxial accelerometer on a 3-way milling vise after calibration and
the fixed frame (xyz). ................................................................................................................... 34
Figure 3-9: Calibration of SF3000L accelerometer system when the initial estimations of biases are
0V: experiment and simulation results. ........................................................................................ 36
Figure 3-10: Calibration of SF3000L accelerometer system when the initial estimations of biases are
10V: experiment and simulation results. ...................................................................................... 36
Figure 3-11: Measurements with the SF3000L triaxial accelerometer system: (a) acceleration
measurements and (b) tilt angle measurements using (3-18). ...................................................... 37
Figure 3-12: Error comparison among the acceleration RMS error, the maximum acceleration error,
and the margin of error of the SF3000L triaxial accelerometer system. ...................................... 38
Figure 3-13: Triaxial accelerometer which consists of two biaxial accelerometers, the sensor axes
(XY), and the fixed frame axes (xy). ............................................................................................ 39
Figure 3-14: Calibration of the Mechworks triaxial accelerometer system when the initial estimations
of biases are 2.5V: experiment and simulation results. ................................................................ 40
Figure 3-15: Calibration of the Mechworks triaxial accelerometer system when the initial estimations
of biases are 10V: experiment and simulation results. ................................................................. 40
ix
Figure 3-16: Measurements with the Mechworks triaxial accelerometer: (a) acceleration
measurements and (b) tilt angle measurements using (3-18). ...................................................... 41
Figure 3-17: Error comparison among the acceleration RMS error, the maximum acceleration error,
and the margins of error of the MA-A202 and MA-A210 accelerometer system. ....................... 42
Figure 4-1: Overview of the position/orientation sensing system. ....................................................... 45
Figure 4-2: Fastening tool, the tool holder, and a sensor that consists of a triaxial magnetometer and
an IMU. The tool frame is labeled xyz and the fixed frame is labeled XYZ. ............................. 53
Figure 4-3: Comparison study between the fastening action and running the tool in the air: (a)
magnitude of magnetic field and (b) magnitude of acceleration. ................................................. 55
Figure 4-4: Workpiece with four bolts. Bolt 1 and Bolt 3 have the same orientation and Bolt 2 and
Bolt 4 have the same orientation. ................................................................................................. 59
Figure 4-5: (a) Magnitude of magnetic field, (b) magnitude of acceleration, and (c) identified fastened
bolt number by the proposed method. .......................................................................................... 60
Figure 4-6: Velocity comparison in each axis among the true values measured with an ultrasonic
sensor, the calculated values using the conventional navigation equations, and the calculated
values using the proposed method. ............................................................................................... 61
Figure 4-7: Position comparison in each axis between the true values measured with an ultrasonic
position sensor and the calculated values using the proposed method (a) entire time span and (b)
magnified when Bolt 1 is fastened – before and after ZUPT, and after the position correction
using (4-33). ................................................................................................................................. 62
Figure 5-1: Two fastening tools are attached to a string balancer on the centre of mass of the tools .. 65
Figure 5-2: Overview of the fastening tool tracking system ................................................................ 66
Figure 5-3: Local fixed frame and tool frame: the Z-axis of the local fixed frame is the opposite
direction of the gravity vector and the z-axis of the tool frame is along the bolt socket axis. ..... 67
Figure 5-4: Membership functions of the fuzzy expert system for the tilt angle correction algorithm.
...................................................................................................................................................... 72
Figure 5-5: Possible acceleration measurement signatures of the fastening tool used in different
scenarios: a) fastening action, b) base excitation, c) hand vibration, and d) normal movements. 73
Figure 5-6: Fastened bolt identification process................................................................................... 74
Figure 5-7: Position sensor error envelope while a tool fastens a bolt. The position of the tool tip
coincides with one of the bolts. .................................................................................................... 75
x
Figure 5-8: Membership degree functions of a) calculated tool tip position error, b) run-time, and c)
the output for Bolt n. .................................................................................................................... 76
Figure 5-9: Fastened bolt identification when position error is the determining factor. ...................... 76
Figure 5-10: Membership degree functions of a) calculated tool tip position error, b) position sensor
error, and c) output for Bolt n. ...................................................................................................... 77
Figure 5-11: Testbed for the lab experiment. ....................................................................................... 80
Figure 5-12: Tool tracking results a) with the intelligent system and b) without the intelligent system.
...................................................................................................................................................... 80
Figure 6-1: Outline of the proposed method. ....................................................................................... 84
Figure 6-2: The proposed hybrid system and the true orientation measurement system. .................... 93
Figure 6-3: True (a) position and (b) orientation measurements using Optotrak. ................................ 94
Figure 6-4: Orientation errors using (a) EKF and (b) the proposed method with 20 particles. ............ 95
Figure 6-5: RMS rotation matrix error using (a) EKF and (b) the proposed method with 20 particles.
...................................................................................................................................................... 95
Figure 6-6: Euler angle errors using the proposed method when the initial orientation is known (a) 5
particles, (b) 20 particles, and (c) 80 particles. ............................................................................. 96
Figure 6-7: RMS rotation matrix errors using the proposed method when the initial orientation is
known (a) 5 particles, (b) 20 particles, and (c) 80 particles. ........................................................ 96
Figure 6-8: Euler angle errors when the initial orientation is unknown: (a) 5 particles, (b) 20 particles,
and (c) 80 particles. ...................................................................................................................... 97
Figure 6-9: First 15 seconds of Figure 6-8 (c), the orientation error with 80 particles when the initial
orientation is unknown. ................................................................................................................ 99
Figure 6-10: Euler angle errors using the proposed method with 20 particles when the initial
orientation is unknown and accelerometer measurements are not used to estimate the roll and
pitch angles, (a) full range and (b) range from 0° to 6°. ............................................................... 99
Figure 6-11: Euler angle errors of the proposed method with (a) 5 particles, (b) 20 particles, and (c)
80 particles when Gaussian noise is added to the position measurements. ................................ 100
Figure 6-12: RMS position error when Gaussian noise is added to the position measurements: (a)
added Gaussian noise using no filter (b) using an EKF (c) using the proposed filter with 5
particles, (d) using the proposed filter with 20 particles, and (e) using the proposed filter with 80
particles. ..................................................................................................................................... 101
xi
Figure 6-13: Euler angle error of the proposed method with (a) 5 particles, (b) 20 particles, and (c) 80
particles when random noise is added to position measurements. ............................................. 102
Figure 6-14: RMS position error when random noise is added to the position measurements: (a) added
random noise using no filter (b) using the proposed filter with 5 particles, (c) using the proposed
filter with 20 particles, and (d) using the proposed filter with 80 particles. ............................... 103
Figure 6-15: Euler angle error with the proposed method with (a) 5 particles, (b) 20 particles, and (c)
80 particles when sinusoidal position errors with an attitude of 0.007 m is added to the position
measurements. ............................................................................................................................ 104
Figure 6-16: The angular velocity (°/sec) of each axis: (a) the entire time span and (b) between 65
second and 70 second. ................................................................................................................ 109
Figure 6-17: Tracking the fastened bolts when the initial orientation is unknown. ........................... 112
Figure 6-18: Fastened bolt detecting sequence. Square blocks indicate possible fastened bolts and
two hexagrams on the bolt signify the fastened bolts. ................................................................ 114
Figure 6-19: Fastened bolt detecting sequence. Square blocks indicate possible fastened bolts and
two hexagrams on the bolt signify the fastened bolts. ................................................................ 116
Figure A-1: Rotation of vector v by the angle of 2θ about vector q. .................................................. 124
Figure A-2: Rotated vector component vn before and after rotation. ................................................. 125
Figure B-3: The calibration of z-axis gyro using a reference block. .................................................. 127
xii
List of Tables
Table 1-1: Position sensors and their advantages and limitations. ......................................................... 7
Table 3-1: Simulation results with different gains and biases. ............................................................. 30
Table 4-1: Position Error using the proposed method before and after position correction. ................ 63
Table 5-1: Fuzzy rules to identify the stationary state of the tool. ....................................................... 72
Table 5-2: Rules for the fastening tool tracking system in linguistic terms. ........................................ 78
Table 5-3: Bolt positions shown in Figure 5-11. .................................................................................. 80
Table 5-4: Position error comparison between with and without the intelligent system. ..................... 81
Table 6-1: Summary of the proposed particle filtering technique. ....................................................... 88
Table 6-2: Number of particles and their processing time. ................................................................ 101
Table 6-3: Position error comparison between the KF-based system and the PF-KF-based system.. 108
Table 6-4: Position error comparison between the KF-based system and the PF-KF-based system
when the angular velocity components have high errors............................................................ 109
Table 6-5: Total position error comparison among the PF-KF-based system with different numbers of
particles and different covariance when the angular velocity components have high errors. .... 110
Table 6-6: Bolt locations. ................................................................................................................... 113
Table 7-1: Advantages and disadvantages of the system presented in this thesis. ............................. 119
xiii
Chapter 1
Introduction
1.1 Motivation
Traditionally, motion tracking systems have been used for outdoor applications such as vehicle
and missile tracking [1]-[3] because traditional inertial sensors were bulky and heavy. Recently,
various motion tracking systems have been developed and used for indoor applications such as
manufacturing [4], human supporting systems [5], [6], and rehabilitation [7]. This thesis describes
developments of tool tracking systems that identify fastened bolts as a quality control system.
Quality control has been a primary focus in the automotive industry. However, many automotive
parts are still produced without any quality control process. For instance, the fastening process is
necessary in various assembly lines such as engine mount, air bags, and seat assembly. This process
requires fastening bolts in the right place with the right amount of torque. Currently, human operators
can set the output torque of the tool and monitor the fastening process to ensure that each bolt is
fastened through the application of a torque value within the desired range. However, monitoring the
torque values alone does not guarantee that each bolt is fastened in the right places because operators
can make mistakes by not fastening all the bolts or fastening a bolt in the wrong sequence. Therefore,
these assembly flaws in the fastening process should be significantly reduced to produce safer and
higher quality automotive parts. To eliminate any potential mistakes and correctly fasten bolts, a
quality control system that tracks the location of the tool tip where bolts are placed during fastening is
required.
Currently, only two fastening tool position tracking devices are available in the market; namely,
SmartArm and UOS-100. SmartArm from PINpoint Information Systems Inc. relies on a passive
robotic arm without an actuator [8]. SmartArm consists of various links, and each link has an encoder
that tracks the angle of the link. When an operator moves a tool in the workspace, the encoders of the
passive robot arm track the motion of each link, and the position and the orientation of the tool are
calculated by using kinematics and the geometry of the robot. This passive robot arm can accurately
track the position of the tool tip, but it can also easily limit the movement of the operators working on
the assembly process.
The other product, UOS-100 from Pepperl+Fuchs, uses ultrasonic transducers to track the position
of the tool tip [9]. This system consists of one ultrasonic emitter, at least three receivers, and one
1
control interface unit. The ultrasonic emitter is attached to the tip of a fastening tool, and the
ultrasonic receivers are placed on three different fixed locations such as ceilings. Since this system
locates the ultrasonic emitter by triangulation method, ultrasonic receivers should be placed some
distance apart from each other. By measuring the distance between one emitter and three receivers,
the location of the tool tip in 3D space (x, y, and z) is determined. The advantage of this system is
that it does not interfere with the movements of the operator because only a small light-weighted
ultrasonic emitter is attached to the tool tip. However, an ultrasonic sensor has limited applications
due to maximum emitting angle, reflection, occlusions [10], and sound sensitivity [11]. Since the
ultrasonic position sensor uses triangulation method to locate the 3D position of the ultrasonic
emitter, the lines of sight between the emitter and the receivers are critical. For example, when the
receivers are installed on flat ceilings, UOS-100 can only detect the fasteners that have close to zero
tilt angles in order to obtain the lines of sight. When at least three lines of sight are not secure, the
system fails to locate the position of the tool tip. In addition, UOS-100 is not practical for a noisy
factory environment because of its sensitivity to high frequency acoustics such as metal beating.
In summary, the tracking systems that make use of those two products are limited. Therefore, a
compact tool tracking system that has high dexterity and robustness is needed for automated tracking
and the quality inspection of parts assembled by bolt fastening actions.
1.2 Literature Review
1.2.1 IMU Calibration
Accelerometers and gyros include several errors such as nonlinearity, gain error, and bias, as a
result of temperature change, input voltage and aging [12]-[14].
Nonlinearity is not usually
compensated for, especially for a low cost IMU, because the process of modeling nonlinearity is
complex and the resultant error is often very small. Thus, IMU calibration typically deals with
estimating gains and biases. The gain and bias values depend significantly on temperature as shown
in [12] and [15]. Even with the temperature error compensation, the IMU error depends on the input
voltage. Therefore, the input voltage is usually controlled by a voltage regulator. However, the
supplied voltage varies even if a voltage regulator is used [12]. Therefore, when an IMU is powered,
it is commonly observed that the current gains and biases differ slightly from the previous values
2
even if the IMU is powered off and immediately powered on. To reduce the gain and bias errors, an
IMU should be calibrated whenever it is powered on.
Due to the need for frequent recalibration of an IMU, the calibration method should follow a
simple procedure and must provide a high accuracy.
Section 1.2.1.1 reviews the calibration
techniques of a triaxial accelerometer, and Section 1.2.1.2 provides the background of triaxial gyro
calibration techniques.
1.2.1.1 Triaxial Accelerometer Calibration Techniques
The conventional method of triaxial accelerometer calibration involves rotating an accelerometer
at known tilt angles [16]-[18] to use the gravitational vector as a reference. To achieve accurate
results with this method, the tilt angles must be precisely measured. Calibration methods with an
external device such as an actuator [19], [20] or a position sensor [21], [22] can accurately compute
the gains and biases, but such a device is not usually available outside of a laboratory. Accelerometer
calibration methods that do not require known tilt angles or an external device have been presented in
[23]-[28]. The calibration method in [23] continuously estimates gains and biases of a triaxial
accelerometer to calculate the tilt angle of human body parts. This method requires prior knowledge
of the frequencies of human body movements, and assumes that the acceleration of a body segment
has a zero mean to calibrate a triaxial accelerometer. However, the accuracy of this method depends
on how true the assumptions are.
For example, this method has a higher accuracy when the
accelerometer is attached to the pelvis rather than the trunk. Other methods use a least squares
estimation to calculate the gains, biases, and misalignment errors [24], [25]. The aforementioned
method allows redundant tilt angle measurements to achieve more accurate calibration results.
However, Syed et al. [25] have stated that the initial estimation of the gains and biases need to be
close to the true values to converge to reasonable gains and biases to use the method in [24]. To find
the rough estimates of gains and biases, a calibration procedure in [25] roughly aligns the three axes
of the sensor with the gravity vector once positively and once negatively. Calibration methods rely
on the Taylor series expansion up to the first order term to linearize the nonlinear mathematical model
of the gains and biases are presented in [26]-[28]. Lai et al. [26] have reported a method to find three
gains and three biases of a triaxial accelerometer by placing it in six randomly chosen tilt angles in a
stationary state. The experimental results show that this method usually requires five iterative steps
to estimate the gains and biases. Lai et al. [26] have also stated that this method also requires the
3
initial estimations of the gains and biases so that the true values do not diverge from the correct
solution.
1.2.1.2 Gyro Calibration Techniques
The conventional calibration method rotates each gyro about its axis at various constant angular
velocities, and then, the relationship between the gyro outputs and the angular velocity measurement
are established [18]. To achieve accurate calibration results with this method, the rotated axis must
be perfectly parallel to the calibrating gyro axis. Since this method requires correct angular velocity
measurements, a turntable which can measure angular velocity is used for calibration in [24], [29].
However, the procedures of these methods are inconvenient and time consuming. The calibration
method that utilizes an optical position sensor with three light-emitting-diodes (LEDs) has presented a
faster and simpler procedure [21]. This method can calibrate an IMU by randomly moving the sensor
for half a minute. Then, by using the orientation measurements calculated from three LED positions,
the IMU is calibrated. Although this method is easy to use, the optical sensor is relatively large and
expensive. Consequently, this method is not feasible outside the laboratory. Another calibration
method [16] places the gyro stationary to estimate the bias and rotates the gyro about its rotation axis
360° to calibrate the gain.
1.2.2 Position and Orientation Computation Using an IMU
An IMU can be used to estimate orientation [30]–[32] and position [33]. Assuming that the local
gravity vector is perfectly known at all times, the position and the orientation of an object can be
accurately calculated by numerically integrating the linear acceleration and angular velocity measured
by an ideal IMU. However, an ideal IMU that has continuous and perfectly accurate measurements
does not exist in the real world. Therefore, the position error increases over time due to the numerical
integration of corrupted inertial sensor data. Figure 1-1 depicts the position error simulations of
different grades of stationary IMUs with small biases [11]. The simulations indicate that the position
error reaches 450 mm after 7 seconds with a commercial grade IMU, 10 seconds with a tactical grade
IMU, and 90 seconds with a navigation grade IMU. Figure 1-1 shows that position tracking systems
using an IMU only are not reliable for an extended period of time. In order to estimate the position,
the acceleration needs to be integrated twice and the angular velocity needs to be integrated once.
However, to estimate orientation, the angular velocity needs to be integrated once. This means the
4
orientation error grows at a much slower rate than the position error. Therefore, an IMU is often used
as an orientation sensor, otherwise is usually hybridized with another sensor to estimate the position
[34], [35].
Figure 1-1: Position error simulations of different grades of IMUs [11]
Using an IMU as an orientation sensor is still a challenge because the orientation calculation drifts
over time due to the integration of gyro errors [36]. In order to overcome this problem, three
accelerometers of an IMU are used as a tilt angle sensor because the accelerometer can find the tilt
angle without any integration step. An orientation correction method using a triaxial accelerometer is
described in [37], but the experimental results of this method show a high orientation error. Many
researchers have employed the KF to combine the tilt angles from accelerometer measurements with
the orientation calculation from gyro measurements [38]-[40]. Rehbinder and Hu [40] have utilized
gyros to find the angular position of a robot and correct the tilt angle with accelerometers when the
robot does not accelerate. Fuzzy expert systems are chosen to detect the static state of an object to
correct the tilt angles [41]-[43].
Although these techniques can limit the drift in the roll and pitch angles, they cannot correct the
yaw angle which does not depend on the tilt angles. In order to correct the yaw angle, three
5
magnetometers are used to measure the magnetic field of the earth [44], [45]. However, the magnetic
field of the earth can be corrupted by ferrous materials [46] and the magnetic field can be generated
by electronic devices. Bachmann at el. [47] have reported that when the ferrous material is less than
two feet from a magnetometer, the error due to the distortion of the magnetic field can be significant.
To reduce the distortion of the magnetic field of the earth, methods that combine IMU outputs with
magnetometers using a KF have been proposed [48]-[50]. The accuracy of such methods decreases if
the disturbance has a similar low bandwidth to that of the angular drift encountered by the gyro error
or if the magnetic field is constantly disturbed.
1.2.3 Position Sensors
Various position sensing technologies are available and can be categorized as follows: (i) visualbased, (ii) non-visual-based, and (iii) encoder-based.
Each group has its own advantages and
limitations.
Visual-based position sensors require a detecting system to be in the line of sight of the object or
the marker to obtain measurements (e.g. infrared, camera, and ultrasonic position sensor). An
infrared position tracking system can achieve less than 1 mm root mean square (RMS) error when the
marker is less than 2 m from the sensor [51]-[54]. However, an accurate infrared position sensor such
as Optotrak is very expensive (over $150,000). The accuracy of a camera-based position sensor
depends on various factors such as the distance between the object and the camera(s). Thus, the
position error range varies but [55] shows that stereo vision system can achieve a 5 mm error.
However, the camera-based position estimation techniques still require further improvement [56] and
a complex calibration procedure [57]. In addition, the accuracy of an ultrasonic position sensor varies
depending on the distance between the marker and the receiver. The ultrasonic-based fastening tool
tracking system, UOS-100, has a position error less than 10 mm. However, the ultrasonic position
sensors have performance issues related to reflections, occlusions, and sound sensitivity as discussed
in Section 1.1.
Non-visual-based position sensors do not require lines of sight, but usually have lower accuracy.
Non-visual-based position sensors include IMU, magnetic position sensor [58], radio frequency (RF)
[59]-[61], and ultra-wideband (UWB) [62]. Magnetic position sensors can achieve accuracy better
than a 10 mm error [63], [64] when no ferrous material is in the measuring range. However, when
ferrous material is in the measuring range, the magnetic field is distorted [47], and the position error
6
can be significantly increased depending on the ferrous material and the distance between the material
and the sensor [64], [65]. The position estimation using the RF technology does not require lines of
sight, but introduces undesirable errors when occlusions and reflections occur [66], [67]. UWB is
suitable for in-door tracking applications such as body tracking or shipping container tracking
because UWB can penetrate various wall materials [68]. However, when a human body obstructs the
line of sight between the tag and the sensor, a UWB position sensor cannot locate the tag due to the
signal strength attenuation by human tissue [69]. In addition, the UWB signal can be reflected by
metal.
A string-encoder position sensor that can track 3D position has been introduced [70]. This method
uses one encoder and three force sensors to estimate the position of the end-point of the wire that is
connected to the sensor. However, the friction between the wire and the guidance hole creates a
measurement error in the force sensor, and significant noise is observed when the position is
calculated from the force sensor measurements. Also, an product that utilizes three encoders to find
the 3D position is available [71]. This position sensor has a single wire which is connected to three
encoders to find the position of the other end of the wire. This product has a high accuracy and
provides noise-free outputs. However, it still needs a line of sight due to the existence of the string.
The summary of the position sensors and their advantages and limitations are described in Table
1-1. Since each position sensor has its own characteristics, selecting a position sensor depends on the
required accuracy and the environment of the application.
Table 1-1: Position sensors and their advantages and limitations.
Sensor
Accuracy
Advantages
Limitations
RF
3000 mm
No LOS (line of sight)
Poor accuracy, strength attenuation,
reflection
UWB
500 mm
Ability to measure
Poor accuracy, strength attenuation,
behind walls
reflection
Magnetic
10 mm
No LOS, high accuracy
Ferrous materials limit accuracy
Ultrasonic
10 mm
High accuracy
3 LOS, Noise
Infrared
1 mm
High accuracy
3 LOS, High cost
Camera
5 mm
High accuracy
Complexity, 2 LOS
Encoder
7 mm
High accuracy
LOS, longer length results in poor accuracy
7
1.2.4 Orientation Detection Using Position Sensors
By attaching multiple position markers on the tracking object, drift-free orientation estimation can
be achieved. A multi-antenna GPS receiver has been utilized to find the orientation of a vehicle [72],
[73], and multiple ultrasonic markers have been used to find the orientation [74], [75]. Multiple
position markers are integrated with one IMU to obtain a drift-free position and orientation estimation
[76], [77]. The position and orientation can be estimated more accurately by hybridizing an IMU
with a position sensor. In addition, an IMU is used to estimate position and orientation when the
position sensor data are not available. When position sensors are utilized to estimate the orientation,
the position markers should be attached to a rigid body, and the markers should be located some
distance apart from each other to obtain meaningful orientation measurements.
However, this
requirement is not feasible in many applications due to small object size or the restrictions of
applications.
1.3 Thesis Overview
The primary objective of this thesis is to develop framework for intelligent remote
position/orientation systems. Since each application has its own needs and limitations, the newly
developed framework requires specific design considerations in relation to the application. The
immediate goal of this research is to develop tool tracking systems that can identify the fastened bolt
for an automotive manufacturing environment. The proposed methods integrate an IMU with an
additional sensor to identify the fastened bolt. A MEMS IMU is chosen for this research because it is
small and lightweight. Therefore, it can be attached to a tool without limiting the movement of the
operator. This thesis consists of seven chapters.
In Chapter 2, various hybridization techniques using variants of the Bayesian filter are described
as well as the fundamental concepts of fuzzy expert system.
Chapter 3 describes a novel triaxial accelerometer calibration technique that does not require any
external sensor. The calibration parameters of an accelerometer change slightly whenever the sensor
is powered on or when the temperature of the sensor is changed. Since the presented calibration
method has a very simple calibration procedure and a high accuracy, it can easily be applied by
operators in the automotive industry.
Chapter 4 presents a fastened bolt tracking system using an IMU and a triaxial magnetometer.
This chapter describes how to calculate the position and orientation by using an IMU. Also, an expert
8
system that identifies stationary state, fastening action, and initial position is presented. The expert
system also corrects or reduces position and orientation errors.
Chapter 5 presents a fastening tool tracking system by using an IMU and an encoder-based
position sensor. This tracking system hybridizes one position sensor and one IMU to estimate
position and orientation. In addition, a fuzzy expert system is developed to identify the stationary
state and the fastened bolts.
Chapter 6 presents a hybridization technique which combines a PF and a KF.
This novel
technique estimates orientation using a PF and estimates the position using a KF. An extensive
analysis of the experiments is conducted. This novel method is tested and the results are compared
with those using the KF-based method in Chapter 5.
1.4 Contributions
The contribution of this research is as follows.
•
Development of a novel triaxial accelerometer calibration.
•
Development of a tool tracking system by using an IMU and a triaxial magnetometer.
•
Design of an expert system that identifies the fastening action, steady state, and orientation
correction.
•
Design a fuzzy expert system that identifies the stationary state and fastened bolt
identification.
•
Development of a tool tracking system using an IMU and encoder-based position sensor.
•
Development of the framework of a position/orientation tracking system that combines the
PF and the KF.
•
Development of a tool tracking system that utilizes the KF-PF combination and an expert
system.
9
Chapter 2
Theoretical Preliminaries
2.1 Hybridization Techniques
Even for applications that require only position estimation, many recent tracking systems depend
on a hybrid hardware consisting of one IMU and one position sensor to improve the position
estimation accuracy rather than using a standalone position sensor. When an IMU is integrated with a
position sensor, the drawbacks of each sensor are accounted for, and more accurate state estimations
can be achieved. To integrate one position sensor with one IMU, the variants of the Bayes filter [78],
[79] such as the KF [80], [81] or the PF [82], [83] are widely used. A KF is an optimum observer that
estimates the states of linear Gaussian state space models. KF and its variants such as the extended
Kalman filter (EKF) [84], unscented KF [85], and complementary KF [86], [87] are the most
commonly used filtering techniques to integrate an IMU with a position sensor. When the model is
highly nonlinear or the noise distribution is non-Gaussian, a PF is more suitable because a PF does
not require the state space model to be linear nor assume the noise is zero mean Gaussian. In
addition, even when the initial states are unknown, the states typically converge to the correct values
if enough number of particles is used. A PF approximates the posterior with a set of state samples,
called particles, instead of assuming that the posteriors are Gaussian at every time step. As the
number of particles increases, the approximated posteriors get closer to the true posterior, which
offers more accurate estimation at the cost of higher computational complexity [88], [89]. To reduce
the computational complexity, the linear Gaussian part of the system can be solved by using a KF
while the remaining part is solved by using a PF [90]. This combination provides improved results
even with a smaller number of particles.
2.1.1 Bayes Filter
For a system identification purpose, the following dynamic state space model is considered:
xk = f k ( xk −1 , u k −1 , bk −1 )
(2-1)
z k = hk ( xk , d k ) ,
(2-2)
10
where subscript k represents the iteration number or time t k , xk is the state, f k is a state transition
function from time t k −1 to time t k , u k −1 is the deterministic input, bk −1 is the process noise, z k is the
measurement, d k is the measurement noise, and hk is a measurement function. The complete
solution of the current state ( xk ) is presented through the posterior probability density function (PDF)
when all the measurements up to the current time instant ( z1:k ) and all the inputs up to the previous
time instant ( u0:k −1 ) are given. Bayes filter calculates the posterior PDF, p( x k | z1:k , u 0:k −1 ) , in two
steps: (i) prediction and (ii) update. Using the Bayes rule and the Markov property, that is if the
current state is known, the future state is independent of the past states, the prediction and update step
can be formulated as
Prediction:
p( xk | z1:k −1 , u0:k −1 )
=
∫ p( x
k
| xk −1 , u k −1 ) ⋅ p( xk −1 | z1:k −1 , u0:k −2 ) ⋅ dxk −1
(2-3)
Update:
p( xk | z1:k , u0:k −1 ) =
p ( z k | xk ) ⋅ p( xk | z1:k −1 , u0:k −1 )
.
p ( z k | z1: k −1 , u0:k −1 )
(2-4)
p( z k | xk ) is the likelihood, p( xk | z1:k −1 , u 0:k −1 ) is the prior, and p( z k | z1:k −1 , u0:k −1 ) is a normalizing
factor. In order to construct the posterior PDF, the prior must be available including the initial PDF,
p( x0 ) .
A Bayes filter requires integration over the state space, which is often impossible to calculate
analytically. In some cases, the posterior distribution can be analytically calculated such as the linear
Gaussian state space model (i.e., KF). When the analytical computation is not feasible, the posterior
density is approximated by using estimators such as a PF.
2.1.2 Kalman Filter [91]
The KF presents an optimal solution of a Bayes filter by assuming that the posterior density is
Gaussian. In order for the posteriors to be Gaussian at every time step, the following conditions must
be satisfied [92], [93]:
11
•
The initial PDF is Gaussian.
•
f k ( xk −1 , u k −1 , bk −1 ) is a linear function of xk −1 and uk −1 with added Gaussian noise.
•
hk ( x k , d k ) is a linear function of xk with added Gaussian noise.
By using these assumptions, (2-1) and (2-2) become:
xk = Φ k ⋅ xk −1 + Γk ⋅ u k −1 + bk −1 ,
(2-5)
zk = H k ⋅ x k + d k ,
(2-6)
where Φ k is the system transition matrix from time t k −1 to time tk , Γk is the input matrix, H k is the
measurement matrix. It is assumed that the process noise and the measurement noise have zero-mean
Gaussian distributions and they are uncorrelated. In addition, the covariance of process noise ( Qk )
and the covariance of measurement noise ( Rk ) at each time step are known as well as the initial state
( x0 ) and initial covariance ( P̂0 ).
The probability density distributions of both the predicted state and the measurement are Gaussian.
Since the estimated state ( x̂k ) is the combination of the two probability density distributions, the
estimated state is also Gaussian and has a linear relation of predicted state ~
xk and measurement z k as
follows:
xˆ k = Lk ~
xk + K k z k .
(2-7)
The objective of the KF is to find the weights, Lk and K k , that minimize the error covariance. Since
the KF is a special case of the Bayesian filter, the KF also estimates states by using the prediction step
and the update step. When the previous estimated state and input are known, the current state can be
predicted as
~
xk = Φ k ⋅ xˆ k −1 + Γk ⋅ u k −1 .
(2-8)
In order to predict the error covariance, the error must be predicted. The predicted error is expressed
as
12
~
ek = ~
x k − x k = Φ k xˆ k −1 + Γk u k −1 − x k
= Φ k ( x k −1 + eˆ k −1 ) + Γk u k −1 − x k
= (Φ k x k −1 + Γk u k −1 + bk −1 ) − bk −1 + Φ k eˆk −1 − x k
= Φ k eˆ k −1 − bk −1
,
(2-9)
where eˆk −1 is the estimated error at time t k −1 . Then, the prediction covariance is calculated as
~
T
Pk = E[~
ek e~k ]
= E[(Φ k eˆk −1 − bk −1 )(Φ k eˆk −1 − bk −1 ) T ]
=
T
Φ k E[eˆk −1eˆk −1
T
]Φ k +
T
E[bk −1bk −1
.
T
] − Φ k E[eˆk −1bk −1
]−
T
E[bk −1eˆk −1
]Φ k
(2-10)
T
Since the estimated error and process noise are uncorrelated, E[eˆk −1bk −1T ] = E[bk −1eˆk −1T ] = 0 .
Therefore, the prediction error covariance becomes
~
T
Pk = Φ k Pˆk −1Φ k + Qk −1 ,
(2-11)
where Pˆk −1 is the estimated error covariance at time t k −1 , which is defined as
T
Pˆk −1 = E (eˆk −1eˆk −1 ) .
(2-12)
When the current measurement is available, the predicted state and error covariance is updated.
From (2-7), the estimated error ( êk ) is calculated as:
eˆ k = xˆ k − x k = Lk ~
x + K k z k − xk
= Lk ( x k + ~
ek ) + K k z k − x k
.
= Lk e~k + Lk x k + K k (H k x k + d k ) − x k
= L e~ + ( L + K H − I ) x + K d
k k
k
k
k
k
k
(2-13)
k
In order to minimize the estimated error covariance, Lk should be defined as
Lk = I − K k H k .
(2-14)
xˆ k = Lk ~
xk + K k z k
= (I − K k H k )~
xk + K k z k .
~
= x + K (z − H ~
x )
(2-15)
Then, the estimated state becomes
k
k
13
k
k
k
In order to estimate the estimated error covariance, the estimated error should be calculated. From
(2-13) and (2-14), the estimated error becomes
eˆk = Lk e~k + ( Lk + K k H k − I ) xk + K k d k
.
= ( I − K k H k )e~k + K k d k
(2-16)
Therefore, the estimated error covariance at time t k becomes
T
Pˆ k = E (eˆk eˆk )
[
[
+ E [(1 − K H
T
= E {(1 − K k H k )~
ek + K k d k }{(1 − K k H k )~
ek + K k d k }
T
T
T
= E (1 − K H )~
e ~
e (1 − K H ) T + K d d K
k
k
k
T
~ T T
~T
k ) ek d k K k + K k d k ek (1 − K k H k )
k k
k
k
k
k
k
k
]
]
]
.
(2-17)
T
T
Since the estimated error and measurement noise are uncorrelated, both E[~
ek d k ] and E[d k ~
ek ] are
zero. Then, (2-17) can be simplified as
~
T
Pˆk = ( I − K k H k ) Pk ( I − K k H k ) T + K k Rk K k .
(2-18)
Now, the weight factor, K k , should be calculated. This weight factor should be chosen so that it
minimizes the estimated error covariance. Eq. (2-18) can be expanded as:
~
−1
Pˆk = [( I − K k H k ) Pk ( I − K k H k )T + K k Rk K k
~
~
~
~
= Pk + K k ( H k Pk H kT + Rk ) K kT − K k H k Pk − ( K k H k Pk )T
.
~
~ T T
~
~ T
T
= Pk + K k ( H k Pk H k ) K k + K k Rk K k − K k H k Pk − ( K k H k Pk )
(2-19)
Since P̂k is a covariance matrix, it should be symmetric and non-negative. Thus, K k should be
chosen so that the trace of the estimated error covariance is minimized. The trace of P̂k is calculated
as
~
~
~
Trace[ Pˆk ] = Trace[ Pk ] + Trace[ K k ( H k Pk H kT ) K kT ] + Trace[ K k Rk K kT ] − 2Trace[ K k H k Pk ] .
(2-20)
To find the weight factor K k that minimizes Trace[Pˆ ] , the derivative of Trace[Pˆ ] with respect to
K k should be zero as
14
~
~
∂Trace[ Pˆk ] ∂Trace[ Pk ] ∂Trace[ K k ( H k Pk H kT ) K kT ] ∂Trace[ K k Rk K kT ] 2∂Trace[ K k Bk ]
=
+
+
−
=0.
∂K k
∂K k
∂K k
∂K k
∂K k
(2-21)
The first term in (2-21) is
~
∂Trace[ Pk ] ∂Trace[Φ k −1 Pˆk −1Φ k −1 + Qk −1 ]
=
=0.
∂K k
∂K k
Since the error covariance matrix and the measurement noise covariance matrix are symmetric, the
second term and third term in (2-21) is
~
T
∂Tracer[ K k ( H k Pk H kT ) K k ]
~
= 2 K k ( H k Pk H kT )
∂K k
T
∂Trace[ K k Rk K k ]
= 2 K k Rk .
∂K k
The last term in (2-21) is
~
2∂Trace[ K k H k Pk ]
~
= 2( H k Pk ) T .
∂K k
Then, (2-21) can be simplified as follows:
~
~
2 K k ( H k Pk H kT + Rk ) − 2( H k Pk )T = 0 .
(2-22)
Then, the weight factor becomes:
~
~
K k = PkT H kT ( H k Pk H kT + Rk ) −1 .
(2-23)
Since the predicted error covariance matrix is symmetric, the weight factor can be written as
~ T
~ T
K k = Pk H k ( H k Pk H k + Rk ) −1 .
Then, (2-18) can be further simplified as follows:
15
(2-24)
~
Pˆk = [( I − K k H k ) Pk ( I − K k H k ) T + K k Rk K kT ]
~
~
~
~
= Pk + K k ( H k Pk H kT + Rk ) K kT − K k H k Pk − ( K k H k Pk ) T
~
~
~
~
~
~
= Pk + [ Pk H T ( H k Pk H kT + R k ) −1 ( H k Pk H kT + Rk ) K kT ] − K k H k Pk − ( K k H k Pk ) T
~ ~
~
~
= Pk + Pk H kT K kT − K k H k Pk − ( K k H k Pk ) T
~
~
= Pk − K k H k Pk
~
= [I − K k ⋅ H k ] ⋅ Pk
(2-25)
In summary, the prediction and update equations of the KF are as follows.
Prediction:
Predicted state:
Predicted covariance:
~
xk = Φ k ⋅ xˆ k −1 + Γk ⋅ u k −1
(2-8)
~
T
Pk = Φ k ⋅ Pˆk −1 ⋅ Φ k + Qk −1
(2-11)
Update:
Kalman gain:
Estimated covariance:
Estimated state:
[
~
~
T
T
K k = Pk ⋅ H k ⋅ H k ⋅ Pk ⋅ H k + Rk
]
−1
(2-24)
~
Pˆk = [I − K k ⋅ H k ]⋅ Pk
(2-25)
xˆ k = ~
xk + K k ⋅ ( z k − H k ⋅ ~
xk )
(2-15)
2.1.3 Sampling Importance Resampling Particle Filter [93]
The PF is a suboptimal solution that approximates the true posterior with a finite number of
random state samples with the corresponding normalized weights.
Then, the posterior density
approximation at time t k is
N
p( xk | z1:k , u0:k −1 ) ≈
∑ w δ (x
i
k
k
− xki ) ,
(2-26)
t =1
where δ (⋅) is the Dirac delta function, N is the number of samples, wki is the normalized weight of the
ith particle at time tk , and xki is the ith state particle at time tk .
16
In order to derive the PF state estimation algorithm, consider the posterior density up to time tk
such that
p( x0:k | z1:k , u0:k −1 ) =
p( z k | xk ) ⋅ p( xk | xk −1 , u k −1 ) ⋅ p ( x0:k −1 | z1: k −1 , u0: k −2 )
.
p ( z k | z1: k −1 , u0: k −1 )
(2-27)
Since it is usually difficult to sample from the posterior density, the importance sampling technique
[94] is used to sample in the PF. When the target density (posterior density in this case) can be
evaluated at any point but is difficult to sample from, samples can be drawn from a known
normalized probability density [ r (x) ], the so-called importance density. To compensate for the
difference between the target density and the importance density, normalized weights, which are the
ratios of the two densities, are assigned to all the particles [92]. The discrete posterior density
approximation up to time t k is expressed as
N
p( x0:k | z1:k , u 0:k −1 ) ≈
∑ w δ (x
i
k
0:k
− x0i :k ) .
(2-28)
t =1
The normalized weight has the following relationship with the target density, p(x) , and the
importance density, r (x) :
wki ∝
∝
p ( x0i :k | z1:k , u 0:k −1 )
r ( x0i :k | z1:k , u 0:k −1 )
p( z k | xki ) ⋅ p ( xki | xki −1 , u k −1 ) ⋅ p( x0i :k −1 | z1: k −1 , u 0: k −2 )
r ( x0i :k | z1:k , u 0:k −1 )
(2-29)
.
The importance density should be chosen so that it can be determined recursively as
r ( x0:k | z1:k , u0:k −1 ) = r ( xk | x0:k −1 , z1:k , u0:k −1 ) ⋅ r ( x0:k −1 | z1: k −1 , u 0:k −2 ) .
(2-30)
When the importance density also satisfies the Markov property same as the target density, (2-29) is
rewritten as
wki ∝
∝
p ( z k | x ki ) ⋅ p( x ki | x ki −1 , u k −1 ) ⋅ p ( x 0i :k −1 | z1: k −1 , u 0: k − 2 )
r ( x ki | x ki −1 , z k , u k −1 ) ⋅ r ( x 0i :k −1 | z1: k −1 , u 0:k − 2 )
p( z k | x ki ) ⋅ p ( x ki | x ki −1 , u k −1 )
r ( x ki | x ki −1 , z k , u k −1 )
17
⋅
wki −1
.
(2-31)
To simplify (2-31), the importance density can be chosen from prior as
r ( xk | xk −1 , z k , u k −1 ) = p ( xk | xk −1 , u k −1 ) .
(2-32)
wki ∝ wki −1 ⋅ p ( z k | xki ) .
(2-33)
Then, (2-31) can be rewritten as
The problem with this type of PF is that only one particle will have high weight (close to unity)
and the remaining particles will have negligible weights (almost zero) when k is high.
This
phenomenon, called a degeneracy problem, is undesirable because the weighted particles do not
represent the true posterior density. In order to avoid this problem, particles can be resampled based
on their weights. Resampling draws more samples from the higher weights and reduces the number
of samples from the lower weights. After resampling, all the particles are assigned the same weight;
thus, the weights at time t k −1 are the same ( wki −1 = 1 / N ). Then, (2-33) becomes
wki ∝ p ( z k | xki ) .
(2-34)
After resampling at time t k , (2-26) can be written as
p( xk | z1:k , u0:k −1 ) ≈
1
N
N
∑δ (x
k
− xki ) .
(2-35)
t =1
Some approaches in the literature propose to calculate the weights based on the “fitness” value
[95], [96] or the “evidence” value [97], [98] of each particle to represent the likelihood in (2-4).
2.2 Fuzzy Expert Systems
Fuzzy expert systems, a branch of artificial intelligence, are very powerful decision-making tools
and are used for quality control algorithms in various industries including textile companies [99],
steel companies [100], and PCB manufacturing [101]. The main components of an expert system are
a knowledge base, reasoning mechanism, and user interface [102]. The knowledge base in a classical
expert system is constructed with facts and rules that are expressed in Boolean logic. Rules are often
expressed in a form of ‘IF A, THEN C’ where A is a set of antecedent conditions, and C is a set of
consequences. In classical crisp logic, the consequence, C, is true when the antecedent, A, is
perfectly satisfied. As discussed in section 1.2, position sensors do not always output reliable
measurement.
As a result, fuzzy expert systems are proposed in various position tracking
18
applications including a GPS/IMU hybrid system [106]-[108] and an Ultrasonic/IMU hybrid sensor
[109] due to the ability to estimate outputs when sensor measurements have uncertainties.
2.2.1 Fuzzy Set
A fuzzy expert system utilizes fuzzy sets [103] and fuzzy logic [104]-[105] and allows for the
computation of a partially true consequence based on how much the antecedent is satisfied. A fuzzy
set, F, is defined on a universe discourse and can be described in terms of the membership function.
The fuzzy membership function can take any value between 0 and 1, where 0 indicates false
(complete non-membership) and 1 indicates true (complete membership). The value between 0 and 1
represents a partial membership to the fuzzy set. The fuzzy set is a generalization of crisp sets where
the governing axioms are relaxed to allow for partial membership. A fuzzy set of generic elements, x ,
and its membership function, µF(x), can be represented as follows:


F = 


∫ µ (x )
∑ µ (x )
F
U
F
x if U is continuous
x if U is discrete
U
DeMorgan’s law, laws of contradiction and the excluded middle are also investigated for these
operations. Motivated by their crisp counterparts, for any two fuzzy sets, A and B, the fuzzy union,
intersection and complement is defined as:
µ AU B ( x ) = max[µ A ( x ), µ B ( x )]
µ AI B ( x ) = min[µ A ( x ), µ B ( x )]
(2-36)
µ A (x ) = 1 − µ A (x )
The fuzzy set theory can be viewed as an extension of the classic crisp set theory where
DeMorgan’s Law holds. However, the laws of contradiction and excluded middle in a crisp set may
not be true for fuzzy operations due to a partial membership. For example, the following can be true
in a fuzzy set:
AU A ≠U
AI A ≠ ∅
19
(2-37)
2.2.2 Fuzzy Logic
By applying the fuzzy set to the classical crisp logic, fuzzy logic is derived. The fundamental
axioms of the fuzzy logic are: (i) each fuzzy proposition has a membership degree between 0 and 1,
(ii) each fuzzy proposition is a collection of linguistic terms and fuzzy operations, (iii) the terms of
fuzzy proposition are defined within the fuzzy set domain, and (iv) the fuzzy logic operators
combining fuzzy propositions are conjunction, disjunction, negation and implication. In fuzzy logic,
a proposition is a combination of terms that are defined within the boundaries of the fuzzy set theory.
A fuzzy rule is also expressed in the form of ‘IF A THEN C’ like the classical crisp logic, but both A
and C have their own membership function. The membership function measures the degree of truth
of the implication.
The fuzzy logic system provides a method of mapping an input space to an output space. The
mapping is achieved by transforming the crisp inputs to the membership values of a fuzzy set. This
process is called fuzzification, and an example is shown in Figure 2-1. When the input value is X1 or
X3, the membership degree becomes 0.5, and when the input value is X2, the membership value is 1.
The membership degrees of the fuzzy set are processed by using the fuzzy inference mechanism and
fuzzy rules incorporated in the rule-base and yield a fuzzy output. This fuzzy output is transformed
back into crisp outputs.
This inverse process of fuzzification is called defuzzification.
Many
defuzzifiers have been proposed and the following five difuzzifiers are widely used: maximum
defuzzifier, mean of maxima defuzzifier, centroid defuzzifier, height defuzzifier, and modified height
Membership Degree
defuzzifier.
1
0.5
0
X1
X2
X3
Figure 2-1: Fuzzification example.
20
In a fuzzy expert system, fuzzy rule-base is a collection of rules based on expert knowledge to
identify the state (or output) of the given system by using the inputs. A fuzzy rule with multiple
antecedents and a single conclusion in Mamdani’s style is described as follows:
IF x1 is Al,1 … x2 is A2,j … xd is Aj,d THEN y is Cl
where x1,…, xd are the input space, and y the output, subscript j is the rule number, j=1,…,d is the
index for the inputs, and Aj,d and C1, are fuzzy sets, and denotes a fuzzy operation such as ‘AND’,
‘OR’. In Mamdani’s rule, the consequence of the rule itself is a fuzzy set. Although each set of
antecedent can have multiple inputs, each set has a single membership degree value.
The
membership degree derived from each antecedent is used to reshape the rule’s output. An example of
an inference mechanism is given in Figure 2-2. For example, The rules are given as follows:
Rule 1: IF x1 is Al,1 AND x2 is Al,2 THEN y1 is max[u1(X1), u2(X2)] and
Rule 2: IF x1 is A2,1 AND x2 is A2,2 THEN y2 is min[u1(X1), u2(X2)].
The membership degree based on Input 1 in Rule 1 is 0.3 and the membership degree based on Input
2 in Rule 1 is 0.5. Since Rule 1 states to find the maximum of the two inputs, the membership degree
of the output of Rule 1 (y1) is 0.5, and the membership function of y1 becomes the shaded shape of
the output of Rule 1.
For Rule 2, the membership based on the output (y2) is 0.4, and the
membership function is shown as the shaded shape of the output in Rule 2. The results of each rule
are added to get the crisp output value through defuzzification. When the centroid defuzzifier is used,
output y1 and output y2 are added and the centre of gravity value is chosen as the output.
21
Membership
Degree
Rule 1
1
1
1
0.5
0.5
0.5
0
0
0
Membership
Degree
Rule 2
y1
1
1
1
0.5
0.5
0.5
0
0
0
y2
x1
x2
Figure 2-2: Inference mechanism.
22
Chapter 3
Accelerometer Calibration Technique
This chapter presents a novel triaxial accelerometer calibration method that does not require any
additional sensor but guarantees a high accuracy. The proposed triaxial accelerometer calibration
method utilizes the gravity vector and the mathematical model of the triaxial accelerometer
calibration parameters; three gains and three biases.
This method only requires the triaxial
accelerometer to be stationary in six different tilt angles to estimate six calibration parameters and
does not require any knowledge of the tilt angles. Since this calibration method does not require any
additional sensor but offers a high accuracy, it is suitable for a variety of applications including
industrial applications.
3.1 Overview
Figure 3-1 summarizes the calibration parameter estimation steps and the process of conversion
from the triaxial accelerometer outputs to accelerations. If the calibration parameters are available,
the accelerometer data are converted from voltage to acceleration, but if the calibration parameters are
not available, the proposed method outputs the accelerometer measurements. The fundamental basis
of the proposed triaxial accelerometer calibration method is that the vector sum of the acceleration
measurement by using a triaxial accelerometer is equal to the gravity vector when the sensor is
stationary. Since a triaxial accelerometer has six unknown calibration parameters, three gains and
three biases, it has to be placed in six different tilt angles to obtain six equations as shown in Figure 32. The stationary state is identified by using an expert system. When the six tilt angles are measured
for calibration, the sensor should be rotated in at least two different axes. If the sensor is rotated in
only one axis, one of the three accelerometers will not be calibrated. When the accelerometer output
readings in six different tilt angles are collected, the six calibration parameters can be estimated from
the derived equations.
When the sensor is stationary, the relationship between the local gravity vector ( g l ) and the
accelerations in X, Y, and Z axis (AX, AY, and AZ, respectively) is
2
2
2
AX + AY + AZ = ( g l ) 2 .
23
(3-1)
Triaxial
accelerometer
outputs
Calibration
parameters
available?
Triaxial
Accelerometer
System
Calibrate
Acceleration outputs
Yes
No
Accelerometer outputs
Calibration Parameters Estimation
Accelerometer
stationary?
Six tilt angles
available?
Yes
No
Estimate
calibration
parameters
Yes
No
Figure 3-1: Flow chart diagram of the proposed triaxial accelerometer calibration method.
z
z
y
z
y
x
y
y
x
z
z
x
y
Gravity
y
x
x
x
z
Figure 3-2: Calibration procedure: the proposed calibration method requires six different tilt
angle measurements to determine six calibration parameters.
The relationship between the accelerometer outputs in each axis ( S axis ) and the true acceleration in
each axis ( Aaxis ) can be written as
S axis = Gaxis ⋅ Aaxis + Baxis ,
(3-2)
where Gaxis is the true gain of each axis and Baxis is the true bias of each axis. Therefore, the squares
of the triaxial accelerometer system outputs are described as
2
2
2
S x + S y + S z = (Gx ⋅ Ax + Bx ) 2 + (G y ⋅ Ay + B y ) 2 + (Gz ⋅ Az + Bz ) 2 .
When the accelerometer is stationary, (3-1) holds, and (3-3) can be expanded as
24
(3-3)
2
2
2
2
2
2
2
2
2
S x + S y + S z = (G x − 1) ⋅ Ax + 2 ⋅ G x ⋅ Ax ⋅ B x + B x
2
+ (G y − 1) ⋅ Ay + 2 ⋅ G y ⋅ Ay ⋅ B y + B y
2
(3-4)
2
+ (G z − 1) ⋅ Az + 2 ⋅ G z ⋅ Az ⋅ B z + B z + ( g l ) 2 .
When all three true gains are unity and all three true biases are zero, the accelerometer system output
of each axis is equal to the acceleration. Otherwise, there should be an error term (Error) due to the
gain error and the bias error where the gain error is defined as the gain minus unity, and the bias error
is the bias minus zero. Thus, (3-4) is represented as
2
2
2
2
S x + S y + S z = g l + Error
2
2
2
(3-5)
2
Error = S x + S y + S z − g l .
By substituting (3-5) into (3-4), the error term can be described as
2
2
2
2
2
2
2
2
Error = (G x − 1) ⋅ Ax + 2 ⋅ G x ⋅ Ax ⋅ B x + B x
+ (G y − 1) ⋅ Ay + 2 ⋅ G y ⋅ Ay ⋅ B y + B y
(3-6)
2
+ (G z − 1) ⋅ Az + 2 ⋅ G z ⋅ Az ⋅ B z + B z .
The error term of (3-6) can be determined from (3-5). However, since the acceleration terms are
unknown, the acceleration terms of (3-6) should be replaced with the known accelerometer system
output terms. Then, (3-6) is rewritten as
2
2
2
2
2
Error = (1 − 1 / G x ) ⋅ S x + 2 ⋅ S x ⋅ B x / G x + (1 − 1 / G y ) ⋅ S y + 2 ⋅ S y ⋅ B y / G y
2
2
2
2
2
2
2
2
2
2
(3-7)
+ (1 − 1 / G z ) ⋅ S z + 2 ⋅ S z ⋅ B z / G z − ( B x / G x + B y / G y + B z / G z ).
2
2
2
2
2
2
Eq. (3-7) has six unknowns, and the last term, ( Bx / Gx + B y / G y + Bz / G z ) , is non-linear and
has six unknowns. Thus, to determine all six unknowns, this problem should be solved by using an
iterative method.
3.2 Iteration Method to Calculate Axes Gains and Biases
The proposed calibration method uses an iterative method to calculate the gains and biases of each
axis of a triaxial accelerometer. To implement an iterative method, (3-2) should be rewritten so that it
fits the method. Thus, (3-2) is rewritten as
25

 k ~
S axis = Gaxis ⋅ Aaxis + Baxis = 
Gaxis ,i  ⋅ Aˆ axis ,k +
 i =0

= Gˆ
⋅ Aˆ
+ Bˆ
,
∏
axis , k
axis , k
k
~
∑B
i =0
axis ,i
,
(3-8)
axis ,k
~
~
where Gaxis ,i represents the calculated gain of each axis at the ith iteration, Baxis ,i represents the
calculated bias of each axis at the ith iteration, Aˆ axis ,k is the estimated acceleration of each axis at the
kth iteration, Gˆ axis ,k is the estimated gain of each axis at the kth iteration, and Bˆ axis ,k is the estimated
bias of each axis at the kth iteration. Given that the true gains are real positive numbers and the true
biases are real numbers, the initial estimated gain of each axis ( Gˆ axis ,0 ) can be chosen from any
positive real number, and the initial estimated bias of each axis ( Bˆ axis ,0 ) can be chosen from any real
number.
When the iterative method converges, the estimated gains and biases at the kth iteration should
match their true counterparts. Thus, the objective of the iterative method is to determine the
~
~
calculated gains of each axis ( Gaxis ,k ) and the calculated biases of each axis ( Baxis ,k ) that satisfy
~
Gaxis = Gˆ axis ,k = Gˆ axis ,k −1 ⋅ G axis ,k
.
~
Baxis = Bˆ axis ,k = Bˆ axis ,k −1 + Baxis ,k
(3-9)
From (3-8), the estimated acceleration at the (k-1)th iteration is
Aˆ axis ,k −1 = ( S axis − Bˆ axis ,k −1 ) / Gˆ axis ,k −1 .
(3-10)
Since the accelerometer outputs are known as well as the previous estimated gains and biases, the
acceleration of each axis at the (k-1)th iteration can be calculated by using (3-10).
When the
accelerometer is stationary, (3-1) holds. If Aˆ axis ,k −1 does not match the true acceleration of each axis
( Aaxis ), an error is encountered. The error term at the (k-1)th iteration is
E k −1 = ( Aˆ X ,k −1 ) 2 + ( Aˆ Y ,k −1 ) 2 + ( Aˆ Z ,k −1 ) 2 − g l
2
2
= ((S X − Bˆ X ,k −1 ) / Gˆ X ,k −1 ) 2 + ((S Y − BˆY ,k −1 ) / Gˆ Y ,k −1 ) 2 + ((S Z − Bˆ Z ,k −1 ) / Gˆ Z ,k −1 ) 2 − g l .
(3-11)
Since all the terms in (3-11) are known, E k −1 can be calculated. By using (3-1), (3-11) is written as
26
E k −1 = ((S X − Bˆ X ,k −1 ) / Gˆ X ,k −1 ) 2 + ((SY − BˆY ,k −1 ) / Gˆ Y ,k −1 ) 2
2
2
2
+ (( S Z − Bˆ Z ,k −1 ) / Gˆ Z ,k −1 ) 2 − ( AX + AY + AZ ).
(3-12)
The true acceleration terms in (3-12) are replaced with the known accelerometer terms from (3-8) and
(3-9). Then, (3-12) can be expanded as
~ 2
~ 2
~
2
E k −1 = (1 − 1 / G X ,k ) ⋅ Aˆ X ,k −1 + 2 ⋅ B X ,k ⋅ Aˆ X ,k −1 /(Gˆ X ,k −1 ⋅ G X ,k )
~ 2
~ 2
~
2
+ (1 − 1 / GY ,k ) ⋅ Aˆ Y ,k −1 + 2 ⋅ BY ,k ⋅ AˆY ,k −1 /(Gˆ Y ,k −1 ⋅ GY ,k )
,
~ 2
~ 2
~
2
+ (1 − 1 / G Z ,k ) ⋅ Aˆ Z ,k −1 + 2 ⋅ BZ ,k ⋅ Aˆ Z ,k −1 /(Gˆ Z ,k −1 ⋅ G Z ,k ) − ε k −1
where ε k −1 = (
(3-13)
~ 2
~ 2
~ 2
B X ,k
BY ,k
BZ , k
+
+
) . Eq. (3-13) has six unknowns and
~
~
~
(Gˆ X ,k −1 ⋅ G X ,k ) 2 (Gˆ Y ,k −1 ⋅ GY ,k ) 2 (Gˆ Z ,k −1 ⋅ GZ ,k ) 2
the last term, ε k −1 , is nonlinear with all six unknowns. However, when the iterations converge, ε k −1
~
~
~
becomes almost zero because the calculated bias terms ( B X ,k , BY ,k , and BZ ,k ) are expected to
converge to zero. By setting ε k −1 zero, (3-13) can be rewritten as
~ 2
~ 2
~
2
E k −1 = (1 − 1 / G X ,k ) ⋅ Aˆ X ,k −1 + 2 ⋅ B X ,k ⋅ Aˆ X ,k −1 /(Gˆ X ,k −1 ⋅ G X ,k )
~ 2
~ 2
~
2
+ (1 − 1 / GY ,k ) ⋅ AˆY ,k −1 + 2 ⋅ BY ,k ⋅ AˆY ,k −1 /(Gˆ Y ,k −1 ⋅ GY ,k ) .
~ 2
~ 2
~
2
+ (1 − 1 / GZ ,k ) ⋅ Aˆ Z ,k −1 + 2 ⋅ BZ ,k ⋅ Aˆ Z ,k −1 /(Gˆ Z ,k −1 ⋅ GZ ,k )
(3-14)
To solve for the six calibration parameters, a triaxial accelerometer should be placed in six
different tilt angles. Then, (3-14) becomes a 6 by 1 matrix such that
[ Errork −1 ] = [ Accelk −1 ] ⋅ [Cal k ]
 E k −11   Aˆ X ,k −112
 E 2  ˆ
2
 k −1   AX ,k −1 2
 E k −1 3  Aˆ X ,k −1 3 2

=ˆ
2
E
4
k
1
−

  AX ,k −1 4
 E k −1 5  Aˆ
52

  X ,k −1 2
 E k −1 6  Aˆ X ,k −1 6
Aˆ Y ,k −112
Aˆ
22
Aˆ Z ,k −112
Aˆ
22
Aˆ X ,k −11
Aˆ
2
X ,k −1
AˆY ,k −11
Aˆ
2
Aˆ Y ,k −1 3 2
Aˆ
42
Aˆ Z ,k −1 32
Aˆ
42
Aˆ X ,k −1 3
Aˆ
4
X ,k −1
AˆY ,k −1 3
Aˆ
4
AˆY ,k −1 5 2
Aˆ
62
Aˆ Z ,k −1 5 2
Aˆ
62
Aˆ X ,k −1 5
Aˆ
6
Aˆ Y ,k −1 5
Aˆ
6
Y ,k −1
Y ,k −1
Y ,k −1
Z , k −1
Z , k −1
Z , k −1
X ,k −1
Y ,k −1
Y ,k −1
Y , k −1
~ 2

Aˆ Z ,k −11  
1 − 1 / G X ,k



~
2
Aˆ Z ,k −1 2 
1 − 1 / GY ,k

~ 2


.
ˆA
1 − 1 / G Z ,k
Z , k −1 3
⋅ ~

~
2
Aˆ Z ,k −1 4  2 ⋅ B X ,k /(Gˆ X ,k −1 ⋅ G X ,k )
~ 2 
  ~
Aˆ Z ,k −1 5  2 ⋅ BY ,k /(Gˆ Y ,k −1 ⋅ GY ,k ) 
~ 2
~
Aˆ Z ,k −1 6  2 ⋅ BZ ,k /(Gˆ Z ,k −1 ⋅ GZ ,k ) 
(3-15)
27
Matrix [ Errork −1 ] can be calculated from (3-11), and Matrix [ Accelk −1 ] can be calculated from (3-10).
Since Matrix [ Errork −1 ] and Matrix [ Accelk −1 ] are known, Matrix [Calk ] is calculated as
[Calk ] = [ Accelk −1 ]−1 ⋅ [ Errork −1 ] .
(3-16)
From Matrix [Calk ] , three calculated gains at the kth iteration should be determined first from the
first three rows. Then, with the square of the calculated gains at the kth iteration and the estimated
gains at the (k-1)th iteration, three calculated biases at the kth iteration can be determined from the last
three rows. The gains, however, must be real positive numbers. To ensure that the calculated gains
are positive real numbers, the calculated gains are calculated as
−0.5
~
−1
G X ,k = 1 − [ Accelk −1 ⋅ Errork −1 ]1 ,
−0.5
~
−1
GY ,k = 1 − [ Accelk −1 ⋅ Errork −1 ]2
,
(3-17)
−0.5
~
−1
GZ ,k = 1 − [ Accelk −1 ⋅ Errork −1 ]3
,
where subscript 1, 2, and 3 represent the row number of [ Accelk −1 ]−1 ⋅ [ Errork −1 ] matrix.
After the six unknowns are determined, the estimated gains and biases at the kth iteration are
obtained from (3-9). This iterative method terminates when the three calculated gains converge to
unity and the three calculated biases converge to zero.
3.3 Numerical Analysis
In experiments, it is difficult to validate the calibration results because the true gains and biases are
unknown.
In addition, accelerometers have many sources of error such as non-linearity,
misalignments, and cross-axis sensitivity. In simulations, however, the true gains and biases can be
defined to allow for validating the iterative calibration technique proposed in subsection 3.1.2 without
the sensor errors. The simulation procedure is depicted in Figure 3-3.
28
Set true gains
(Gaxis) and
true Biases
(Baxis )
Simulate
accelerometer
outputs
Initial
Guesses:
Gˆ axis ,0 = 1
Bˆ
=0
Calibration
axis , 0
Data Generation
Sensor Calibration
Compare final
Gˆ axis ,k with
Gaxis and final
Baxis,k with Baxis
Validation
Figure 3-3: Procedure for the simulation calibration and parameter validation.
The true gains and biases are defined for the simulation, and the accelerometer data are generated
with MATLAB based on the true gains, true biases, gravity, and the movement of the accelerometer.
Then, by using the proposed calibration method, the gains and biases are estimated. Unity gains and
zero biases are chosen to initialize the iterative algorithm. When the final estimated gains and biases
are calculated with the proposed calibration method, they are compared with the true gains and biases.
One hundred simulations were performed with gains between 0.001 and 1000 [V/(m/s2)] and
biases between ±100V. The six stationary tilt angles had at least 2º difference for the simulations
because when the tilt angle differences are less than 1.5º, the gain and bias errors start to increase due
to the computer software precision limitations. Five distinct results of the hundred simulations are
presented in Table 3-1 and the tilt angle data for the five simulations are shown in Figure 3-4. For the
first simulation (Simulation 1 of Table 3-1), all the biases are set to zero. The second simulation
consists of high gains and high biases, and the third simulation contains low gains and low biases.
Simulation 4 consists of a mixture of high and low gains and biases, and Simulation 5 has low gains
and high biases. Simulation 1 converges on the first iteration because the initial estimated biases
match the true biases. In this case, ε 0 in (3-13) becomes zero, and the gains and biases can be
accurately calculated by using (3-14). Simulation 2 to 4 indicate that the biases converge on the first
iteration, and the gains converge on the second iteration. Simulation 5 has high biases and low gains
that create a high ε 0 in (3-13). Due to high ε 0 , Simulation 5 requires more iteration steps than the
other four simulations. For all one hundred simulations, the gains and biases converge to the correct
values within three iteration steps.
29
(b) 90
(c) 90
(d) 90
(e) 90
45
45
45
45
45
0
0
0
0
0
-45
-45
-45
-45
-45
Angle ( °)
(a) 90
-90
0
30
60
-90
0
30
60
-90
0
30
60
-90
0
30
60
-90
0
30
X
Y
Z
60
Time (s)
Figure 3-4: Tilt angle contents of the simulations: tilt angles for (a) Simulation 1, (b) Simulation
2, (c) Simulation 3, (d) Simulation 4, and (e) Simulation 5.
Table 3-1: Simulation results with different gains and biases.
Number of
GX
iterations
[V/(m/s2)]
Simulation 1 Result
True value
0.24
1 - converged
0.24000
Simulation 2 Result
True value
1000
1
999.72
2 - converged
1000.0
Simulation 3 Result
True value
0.001
1
0.010148
2 - converged
0.0010000
Simulation 4 Result
GY
[V/(m/s2)]
GZ
[V/(m/s2)]
712
712.00
32
32.000
0
0.00000
0
0.00000
0
0.00000
500.0
499.86
500.00
400
399.89
400.00
100.0
100.00
100.00
-100
-100.00
-100.00
-25
-25.000
-25.000
0.5
5.0741
0.50000
0.4
4.0593
0.40000
-0.1
-0.10000
-0.10000
0.5
0.50000
0.50000
-1
-1.0000
-1.0000
300
5864.9
1.4
27.369
-11
-11.000
-1.5
-1.5000
80
80.000
2 - converged
0.060000
Simulation 5 Result
300.00
1.4000
-11.000
-1.5000
80.000
True value
1
2
3 - converged
0.002
30.489
0.19244
0.0020000
0.001
15.409
0.096219
0.0010000
-100
-99.155
-100.00
-100.00
100
100.77
100.00
100.00
100
100.17
100.00
100.00
True value
1
0.06
1.1730
0.001
15.191
0.096219
0.0010000
30
BX
(V)
BY
(V)
BZ
(V)
3.4 Experiments
The proposed calibration method was tested with two different triaxial accelerometers. The first
triaxial accelerometer consists of three identical single-axis accelerometers, and the second triaxial
accelerometer is assembled with two different biaxial accelerometers that have different gains. The
experimental system setup is shown in Figure 3-5. One triaxial accelerometer is connected to a data
acquisition card (DAQ) which is connected to a computer. Since both the triaxial accelerometer and
the data acquisition card have their own gains and biases, the proposed calibration method determines
the gains and biases of the combined system (hereafter accelerometer system).
Accelerometer
DAQ
Figure 3-5: Experimental system setup.
The six sides of the accelerometer were placed on a table for calibration because such a procedure
is the easiest way to make a sensor stationary in six different tilt angles and also minimizes the effects
of the sensor errors such as nonlinearity on the calibration parameter calculation. In order to identify
the stationary state and collect the six stationary state sensor data, an expert system is used. When the
accelerometer is stationary, the acceleration in each axis should be constant.
Thus, when the
accelerometer measurements of each axis do not fluctuate with a magnitude that exceeds the
maximum noise level of the accelerometer system for a certain period of time, it is assumed that the
accelerometer is stationary. When the sensor is calibrated by a person, two seconds is a sufficient
time period to identify the stationary state because it is unlikely for a person to maintain the same
acceleration for two seconds unless the sensor is stationary.
Instead of using instantaneous
accelerometer outputs in each axis ( S axis ), the average accelerometer outputs in each axis are used to
minimize the effect of noise in the experiment.
The accurate magnitude of the local gravity vector must be known to solve (3-11). In this
experiment, 9.8036 m/s2 is used for the magnitude of the local gravity vector (Kitchener-Waterloo,
Ontario, Canada) [110]. Unity gains are used for the initial estimated gains because a unit gain is far
31
from the specs of both triaxial accelerometers. For the initial estimated biases, two values are
selected: the bias voltages from the accelerometer specs, which are close to the true biases and 10V
which is far from the biases of both triaxial accelerometers. Since the true gains and biases are not
available in the experiments, the estimated gains and biases cannot be compared with their true
counterparts.
However, if the estimated gains and biases match their true counterparts, the
accelerations measured with an accelerometer at stationary state should match the true accelerations
that are calculated from the tilt angle measurements. Thus, to verify if the final estimated gains and
biases match their true values, the acceleration measurements after calibration are compared with the
accelerations calculated from the gravity vector. The simulations were generated based on the final
estimated gains and biases that are obtained from the experiments with 10V initial estimated biases
for comparison. The experimental test procedure is summarized in Figure 3-6.
Rotate accelerometer
at 0°, 30°, 45°, 60°,
and 90° that are
measured with
inclinometer for
validation
Orient
accelerometer in
six different tilt
angles for
calibration
Data Collection
g l = 9.8036m/s2
Initial guesses
G(axis)0 = 1
B(axis)0 = 10V
or bias on spec
Calibration
Sensor Calibration
- Compare accelerometer
measurements with true
accelerations
- Generate simulations
using gains and biases of
experiments
Validation
Figure 3-6: Procedure for the calibration experiment and parameter validation.
3.4.1 Experiments Using a Triaxial Accelerometer with Three Identical Single-Axis
Accelerometers
A commercially available triaxial accelerometer (Colibrys, SF3000L) is selected for the first set of
experiments.
This accelerometer consists of three almost perfectly perpendicular single-axis
accelerometers that have almost the same gains and biases. After the accelerometer was calibrated by
rotating the sensor in six different tilt angles, the sensor was placed on a milling vise as shown in
Figure 3-7. Then, the sensor was held stationary for a while at 0°, 30°, 45°, 60°, and 90° with respect
to the gravity vector as presented in Figure 3-8 to validate the estimated gains and biases. These
reference tilt angles were measured with a mechanical inclinometer (Hilger & Watts, TB121-1).
32
Accelerometer Y
X
y
x
3-way milling vise
Figure 3-7: Colibrys triaxial accelerometer on a 3-way milling vise: the sensor axes (XY) and
the fixed frame axes (xy).
y
x
z
x
(1) Initial orientation
(2) rotate -30° about x-axis (3) rotate -45° about x-axis
from (1)
from (1)
(4) rotate 30° about x-axis (5) rotate 45° about x-axis (6) rotate 30° about y-axis
from (1)
from (1)
from (1)
33
z
y
(7) rotate 60° about y-axis (8) rotate 90° about y-axis (9) rotate 30° about x-axis
from (1)
from (1)
from (8)
(10) rotate 45° about x-axis (11) rotate -30° about x-axis (12) rotate -45° about x-axis
from (8)
from (8)
from (8)
Figure 3-8: Rotation sequence of a triaxial accelerometer on a 3-way milling vise after
calibration and the fixed frame (xyz).
Since the calibration parameters change whenever an accelerometer is powered on, three
calibration tests are conducted to check if the proposed method can accurately estimate the calibration
parameters in all three cases. Each test used unity initial estimated gains and two different sets of
initial estimated biases: 0V and 10V.
Figure 3-9 illustrates the simulation and experiment
convergence steps of the calibration when the initial estimated biases are close to the true biases of
34
the accelerometer system ( Bˆ axis ,0 = 0V). This figure indicates that the experiment convergence steps
almost agree with the simulation steps.
Figure 3-10 displays the simulation and experiment
convergence steps of calibration when the initial estimated biases are far from the true biases ( Bˆ axis ,0 =
10V). Figure 3-10 shows noticeable mismatches between the simulation steps and the experiment
steps on the first iteration while Figure 3-9 exhibits small mismatches. These mismatches could be
caused by the triaxial accelerometer system error such as nonlinearity and misalignment. The sensor
errors create error in Matrix [ Accelk −1 ] . When ε k −1 in (3-13) is small, the experiment steps almost
match the simulation steps (Figure 3-9) because the multiplication of the inverse of Matrix
[ Accel k − 1 ] and ε k −1 is small. However, when ε k −1 is large, the multiplication of the inverse of
Matrix [ Accelk −1 ] and ε k −1 is large. Thus, the mismatch between the simulations and experiments
becomes significant as shown in Figure 3-10. Due to these sensor errors, the experiments of both
Figure 3-9 and Figure 3-10 converge on the third iteration while the simulations converge on the
second iteration.
The gains and biases of the simulations were generated based on the estimated gains and biases of
the experiments with 10V initial estimated biases. Therefore, Figure 3-10 shows that the gains and
biases of simulations (dotted lines) perfectly match the experimental results (solid lines) after
convergence. Also, Figure 3-9 shows that the experimental results perfectly match the simulation
results when the initial estimated biases are zeros. These results demonstrate that regardless of the
initial estimated bias values, the converged calibration parameters are the same.
35
Gain [V/(m/s2)]
Gx
Gz
Gz
1
1
1
0.5
0.5
0.5
0
0
2
4
0
0
2
4
0
0
2
4
Number of iteration
Bias (V)
Bx
By
0
0
-0.02
-0.03
-0.04
-0.06
Bz
0.12
Experiment 1
Experiment 2
Experiment 3
Simulation 1
Simulation 2
Simulation 3
0.09
0.06
-0.06
0
2
4
-0.09
0.03
0
2
4
0
0
2
4
Number of iteration
Figure 3-9: Calibration of SF3000L accelerometer system when the initial estimations of biases
are 0V: experiment and simulation results.
Gain [V/(m/s2)]
Gx
Gy
Gz
2
2
2
1.5
1.5
1.5
1
1
1
0.5
0.5
0.5
0
0
2
4
0
0
2
4
0
0
2
4
Number of iteration
Bias (V)
Bx
By
Bz
10
10
10
5
5
5
0
-2
0
-2
0
-2
0
0
2
4
0
2
4
2
Experiment 1
Experiment 2
Experiment 3
Simulation 1
Simulation 2
Simulation 3
4
Number of iteration
Figure 3-10: Calibration of SF3000L accelerometer system when the initial estimations of biases
are 10V: experiment and simulation results.
The result of Experiment 3 in Figure 3-9 is analyzed in Figure 3-11. Figure 3-11 (a) illustrates the
acceleration measurements before and after calibration, and Figure 3-11 (b) displays the tilt angle
measurements by the following calculation.
36
θ X = arcsin( AX /( AX 2 + AY 2 + AZ 2 ) 0.5 ) × 180 / π
θ Y = arcsin( AY /( AX 2 + AY 2 + AZ 2 ) 0.5 ) × 180 / π ,
(3-18)
θ Z = arcsin( AZ /( AX 2 + AY 2 + AZ 2 ) 0.5 ) × 180 / π
where θaxis represents the tilt angle of each axis in degree. The horizontal grid lines represent the
accelerations and tilt angles induced by the rotations illustrated in Figure 3-8.
For the first 35 seconds, the accelerometer is calibrated by placing it in six different tilt angles.
During this period, Figure 3-11 (a) demonstrates that the magnitudes of the accelerations are not close
to the magnitude of the gravity vector because the initial estimated gains and biases do not match the
true values. However, after 35 seconds, the accelerometer is calibrated, and the magnitude of the
acceleration almost matches the magnitude of the gravity vector. Figure 3-11 (b) shows that the tilt
angle measurements with the accelerometer after calibration are stationary at 0°, 30°, 45°, 60°, and
90° with the same sequence of rotation displayed in Figure 3-8.
10
8.49
(a)
AY
6.93
AZ
4.9
θX
θY
θZ
60
45
30
Angle (°)
2
Aceeleration (m/s )
90
(b)
AX
0
0
-30
-4.9
-45
-6.93
-60
-8.49
-10
0
500
1000
-90
0
Time (s)
500
1000
Time (s)
Figure 3-11: Measurements with the SF3000L triaxial accelerometer system: (a) acceleration
measurements and (b) tilt angle measurements using (3-18).
A small acceleration error exists due to the errors of the accelerometer and the data acquisition
card. The sources of errors are investigated and compared with the acceleration error after calibration.
SF3000L has a 0.1% nonlinearity error, 0.5° misalignment error, and 0.5% cross-axis sensitivity.
Also, the accelerometer is connected to a computer through a DAQ (Measurement Computing, PCIDAS-1602/16) that contains a nonlinearity error and cross-talk. The specifications of the data
acquisition card indicate that the nonlinearity error is ±5 least significant bits (LSB), and the
maximum cross-talk error is ±2 LSB.
37
The acceleration errors of all three experiments and the error margin of the accelerometer system
are analyzed in Figure 3-12. The figure denotes that the acceleration RMS error is 0.09 m/s2 at 0° and
decreases as the tilt angle increases. This occurs because the effects of the misalignment error and the
cross-axis sensitivity error are the maximum at 0°. The maximum acceleration error is 0.14 m/s2 at 0°
and tends to decrease as the tilt angle increases. Figure 3-12 shows that the maximum acceleration
error of each axis is below the error margin of the accelerometer system.
RMS Error
Maximum Error
Margin of Error
2
Acceleration Error (m/s )
0.24
0.2
0.16
0.12
0.08
0.04
0
0
30
45
60
90
Angle (°)
Figure 3-12: Error comparison among the acceleration RMS error, the maximum acceleration
error, and the margin of error of the SF3000L triaxial accelerometer system.
The experimental results demonstrate that the proposed calibration method accurately estimates
the calibration parameters when three single-axis accelerometers have almost the same gains and
biases. In addition, the experimental results demonstrate that even when the initial estimated biases
are different, the proposed calibration method generates accurate results.
3.4.2 Experiments Using a Triaxial Accelerometer with Different Gains
For the second set of experiments, a triaxial accelerometer is assembled with two biaxial
accelerometers (MA-A202 and MA-A210, Mechworks Systems Inc.) that have different gains. MAA202 has a gain three times higher than MA-A210. This triaxial accelerometer (hereafter referred to
as a Mechworks triaxial accelerometer) is assembled with two axes (X, Y-axis) of MA-A202 and one
axis (Z-axis) of MA-A210 (Figure 3-13).
38
MA-A210
X
Y
y
x
MA-A202
Figure 3-13: Triaxial accelerometer which consists of two biaxial accelerometers, the sensor
axes (XY), and the fixed frame axes (xy).
After the sensor is calibrated, it was held stationary on a milling vise at 0°, 30°, 45°, 60°, and 90°
with respect to the gravity vector as shown in Figure 3-8 for validation. Three calibration tests were
conducted. Each test comprised two different sets of initial estimated biases: 2.5V and 10V. Figure
3-14 shows the simulation and experiment results when the initial estimated biases are close to the
true biases of the accelerometer system ( Bˆ axis ,0 = 2.5V), and Figure 3-15 shows the simulation and
experiment results of the calibration when the initial estimated biases are far from the true biases
( Bˆ axis ,0 = 10V). The experimental results of the Mechworks triaxial accelerometer have the same
trend as the experimental results of SF3000L. Figure 3-14 demonstrates that the experiment steps
agree with the simulation steps while Figure 3-15 does not. In both cases, the experimental results
reveal that the calibration parameters converge to the same values within three iterations.
The experimental results of Experiment 3 in Figure 3-14 are expanded in Figure 3-16. Figure 3-16
(a) shows the acceleration measurements, and Figure 3-16 (b) shows the tilt angle measurements
converted from the acceleration measurements by using (3-18). The first 36 seconds of Figure 3-16
reflect the calibration period where the initial estimated gains of each axis are unity and the initial
estimated biases of each axis are 2.5 V. Since the Mechworks accelerometers have true gains less
than 0.04 V/(m/s2) and the initial estimated biases almost match the true biases, the first 36 seconds
of Figure 3-16 (a) shows the acceleration signal around 0 V with very small fluctuations. However,
after calibration, the two figures in Figure 3-16 illustrate that the acceleration measurements match
the accelerations induced by the rotation sequence illustrated in Figure 3-8.
39
Gain [V/(m/s2)]
Gx
Gy
Gz
1
1
1
0.8
0.8
0.8
0.6
0.6
0.6
0.4
0.4
0.4
0.2
0.2
0.2
0
0
2
4
0
0
2
4
0
0
2
4
Number of iteration
Bx
By
2.5
Bz
2.58
2.5
Experiment 1
Experiment 2
Experiment 3
Simulation 1
Simulation 2
Simulation 3
Bias (V)
2.56
2.49
2.54
2.49
2.52
2.48
0
2
4
2.5
0
2
4
2.48
0
2
4
Number of iteration
Figure 3-14: Calibration of the Mechworks triaxial accelerometer system when the initial
estimations of biases are 2.5V: experiment and simulation results.
Gain [V/(m/s2)]
Gx
Gy
3
3
2
2
1
1
Gz
1
0.8
0.6
0.4
0.2
0
0
2
4
0
0
2
4
0
0
2
4
Number of iteration
Bias (V)
Bx
By
Bz
10
10
10
8
8
8
6
6
4
4
Experiment 1
Experiment 2
Experiment 3
Simulation 1
Simulation 2
Simulation 3
6
4
2
0
2
4
2
2
0
2
4
0
0
2
4
Number of iteration
Figure 3-15: Calibration of the Mechworks triaxial accelerometer system when the initial
estimations of biases are 10V: experiment and simulation results.
40
10
8.49
(a)
AY
6.93
AZ
4.9
θX
θY
θZ
60
45
30
Angle (°)
2
Aceeleration (m/s )
90
(b)
AX
0
0
-30
-4.9
-45
-6.93
-60
-8.49
-10
0
400
800
-90
0
400
800
Time (s)
Time (s)
Figure 3-16: Measurements with the Mechworks triaxial accelerometer: (a) acceleration
measurements and (b) tilt angle measurements using (3-18).
The specifications of the Mechworks accelerometers denote that they have a 0.2% nonlinearity
error, 1° misalignment error, and 2% cross-axis sensitivity. The accelerometers were connected to a
computer through the same data acquisition card that is used to connect the Colibrys triaxial
accelerometer. Figure 3-17 relates the acceleration RMS error, the maximum error, and the margin of
error of MA-A202 and MA-A210 with the data acquisition card.
Figure 3-17 shows that the
acceleration RMS error is 0.10 m/s2 at 0° and tends to decrease as the tilt angle increases. The
maximum acceleration error is 0.16 m/s2 at 0° and tends to decrease as the tilt angle increases. The
maximum error of the system is within the margin of error of both accelerometer systems. The
experimental results of the Mechworks triaxial accelerometer demonstrate that the proposed
calibration method can accurately estimate the gains and biases of a triaxial accelerometer with
different gains.
41
2
Acceleration Error (m/s )
1.2
RMS Error
Maximum Error
MA-A202 Margin of Error
MA-A210 Margin of Error
1
0.8
0.6
0.4
0.2
0
0
30
45
60
90
Angle (°)
Figure 3-17: Error comparison among the acceleration RMS error, the maximum acceleration
error, and the margins of error of the MA-A202 and MA-A210 accelerometer system.
3.5 Conclusion
This chapter presented a novel calibration method that determines the gains and biases of a triaxial
accelerometer by placing the sensor in six different tilt angles. The presented method was developed
by using the mathematical model of gains and biases. To validate the proposed calibration method,
simulations and experiments were performed.
The simulation results demonstrated that the proposed calibration method accurately estimated the
gains and biases within three iterations. Since the true gains and biases were not available in the
experiments, the accelerations measured with an accelerometer after calibration were compared with
the true accelerations calculated from the gravity vector to validate the estimated gains and biases.
The first set of experiments was performed by using a triaxial accelerometer consisting of three
single-axis accelerometers that have almost the same gains and biases. The acceleration RMS error
of the accelerometer after calibration was 0.09 m/s2, and the maximum error was within the margin of
error of the accelerometer system. The second set of experiments was conducted by employing a
triaxial accelerometer that has two different gains and almost the same biases. The acceleration RMS
error of the Mechworks triaxial accelerometer after calibration was 0.10 m/s2, and the maximum error
was within the margin of error of the accelerometer system.
From the simulations and experiments, it is concluded that the proposed calibration method can be
adapted to estimate the calibration parameters accurately. The proposed calibration method follows a
42
very simple procedure and has a low computational cost. In addition, this method does not require
any prior knowledge of the accelerometer calibration parameters nor the six tilt angles that are needed
to implement the iterative approach for the proposed calibration method.
The proposed method is especially useful for a low cost triaxial accelerometer whose initial
estimated gains and biases highly vary from power-on to power-on because the simulation and
experiment results indicate that the gains and biases can be accurately estimated even when the initial
estimated gains and biases are not close to the true value.
43
Chapter 4
Fastened Bolt Tracking System Using an IMU and a Triaxial
Magnetometer with an Expert System
4.1 Proposed Tracking System
In some automotive parts, the bolts are not closely placed or the bolt orientations are different. In
such cases, a cost-effective position/orientation tracking solution with a sufficient accuracy is
desirable. This chapter proposes a cost-effective fastened bolt tracking system by using an IMU and a
triaxial magnetometer.
In order to calculate position from IMU measurements, the accelerations need to be integrated
twice and the angular velocities need to be integrated once. However, since the sensor errors are also
factored in the integration steps, the position errors increase over time as shown in Figure 1-1. In
order to reduce the position error of a down-hole drill, ZUPT was utilized in [111]. ZUPT zeros
velocity whenever the object is stationary, and the position error is reduced by subtracting the
position accumulation error caused by the average velocity error. To achieve high accuracy with
ZUPT, a short time span between the stationary states is desirable. Thus, ZUPT is widely used to
track the location of a person by attaching a low cost IMU on a shoe [112]-[114] because the time
span between each foot step (stationary state) is very short.
For a fastening procedure of the manufacturing environment, an operator picks up a tool from the
tool holder and fastens all the bolts, and then places the tool back to the tool holder. This entire
procedure usually takes less than one minute. The tracking system should be able to detect when the
tool is placed on the tool holder to correct the position and orientation error. In addition, the system
should be able to detect the stationary state and the fastening action to use ZUPT.
The method to accomplish the aforementioned is described in Figure 4-1. First, the IMU must be
calibrated to estimate the position and orientation as accurately as possible. By using the calibrated
IMU measurements, the position and orientation of the tool are calculated. Even if an IMU is wellcalibrated, the position error grows over time due to the integration steps. Therefore, an expert
system is proposed to correct the position, velocity, and orientation errors. By using the corrected
position and orientation of the IMU, the tool tip position and orientation are calculated.
44
Gyroscope
Calibration
Position/Orientation
Calculation
Expert System
IMU
Accelerometer
Stationary state
detection
Initial position
Detection
Calibration
Position and
orientation
correction
Tool tip
position
output
Fastening Action
Detection
Magnetometer
Bolt positions and orientation
Figure 4-1: Overview of the position/orientation sensing system.
4.2 Orientation Representations
An IMU is rigidly strapped down to an object being tracked to measure the linear accelerations
and angular velocities in terms of the object body frame. To define the orientation of the object with
respect to a reference frame, a rotation matrix that describes the transformation from the body frame
to the reference frame is required. In this section, two rotation matrix representations are described:
the direction cosine matrix and the quaternion [115].
4.2.1 Direction Cosine Matrix
A direction cosine matrix consists of three unit vectors that represent three body axes projected on
three reference axes. These three unit vectors form three columns of a direction cosine matrix. The
direction cosine matrix from the tool frame to the local fixed frame is described as follows:
f
tC
c xX

=  c xY
 c xZ

45
c yX
c yY
c yZ
c zX 

c zY  ,
c zZ 
(4-1)
where subscript x, y, z are the orthogonal unit vectors of the tool frame and X, Y, Z are the
corresponding vectors of the reference frame. c mM is the cosine of the angle between vector m in the
body frame and vector M in the body frame.
4.2.2 Quaternion
The quaternion orientation representation is widely used to calculate the orientation of the body
frame with respect to a reference frame. The quaternion representation obtains a rotation matrix with
a single rotation about one axis in space. This method uses a concept of the hyper-complex number
of rank 4: one real number and three imaginary numbers. A quaternion q is written as
q = q0 + qv = q0 + (q1i + q2 j + q3 k ) .
(4-2)
The imaginary numbers follow the right hand rule such that
ij = k , ji = −k , jk = i , kj = −i , ki = j , ik = − j , and i 2 = j 2 = k 2 = −1 .
In order to use quaternion to determine the orientation in a 3D vector space, the real part of a
quaternion v needs to be zero ( v = 0 + v x i + v y j + v z k ). This also means that the real part of a 3D
vector v should be zero once rotated by quaternion q. To satisfy this condition, the 3D vector is
multiplied by quaternion q and the conjugate of quaternion q, denoted as q*, as follows:
v′ = q ⋅ v ⋅ q * ,
(4-3)
where vector v’ is the rotated 3D vector. The conjugate of the quaternion q is defined as
q* = q0 − qv = q0 − q1i − q2 j − q3 k .
(4-4)
The resultant multiplication of (4-3) should form a cosine matrix multiplication by the 3D vector v
expressed as
f
tC
⋅v = q ⋅v⋅q* .
(4-5)
From (4-5), the rotation matrix becomes
 q0 2 + q12 − q2 2 − q3 2

f
2(q1q2 + q0 q3 )
tC = 
 2( q q − q q )
1 3
0 2


q0 − q1 + q2 − q3
2(q2 q3 − q0 q1 )  .
2
2
2
2
2( q2 q3 + q0 q1 ) q0 − q1 − q2 + q3 
2(q1q3 + q0 q2 )
2(q1q2 − q0 q3 )
2
2
When a unit quaternion is used, (4-2) should satisfy
46
2
2
(4-6)
2
2
2
2
q0 + (q1 + q2 + q3 ) = 1 .
(4-7)
q = q0 + qv = cos θ + u sin θ .
(4-8)
Then, (4-2) can be written as
where u is a unit vector ( u = u x i + u y j + u z k ). Then, when the vector v is rotated by angle θ by
applying (4-3), the quaternion components of q become
q0 = cos
θ
2
, q1 = u x ⋅ sin
θ
2
, q2 = u y ⋅ sin
θ
2
, and q3 = u z ⋅ sin
θ
2
.
(4-9)
The rotation angle is divided by a factor of 2 in (4-9) because the vector v was rotated by θ twice:
about both q and q*. The detailed derivation of quaternion orientation representation is given in
Appendix A.
Since the quaternion orientation representation and the direction cosine matrix should match, the
following equality is true:
c xX

f
t C =  c xY
 c xZ

c yX
c yY
c yZ
2
2
2
2
c zX   q0 + q1 − q 2 − q3
 
c zY  =  2(q1 q 2 + q 0 q3 )
c zZ   2(q1 q3 − q0 q 2 )


q 0 − q1 + q 2 − q3
2(q 2 q3 − q 0 q1 )  . (4-10)
2
2
2
2
2(q 2 q3 + q 0 q1 ) q 0 − q1 − q 2 + q3 
2(q1 q 2 − q 0 q3 )
2
2
2
2(q1 q3 + q 0 q 2 )
2
4.3 Position Estimation
Since accelerometers measure accelerations with respect to the inertial frame, the motion
equations using accelerometers are derived from the inertial frame. However, the local fixed frame is
attached to a stationary building which, in fact, moves with respect to the inertial frame in this
application. Therefore, the equations of motion should be expressed with respect to the local fixed
frame. The acceleration of an object with respect to the local fixed frame can be expressed as [116]
f
V& = f A − 2 ⋅ ω e × f V − f [ω e × ω e × R ]+ f g ,
(4-11)
where ωe is the angular velocity of the earth with respect to the inertial frame and R is the location of
the tool from the centre of the earth. In this application, the centripetal force term, ω e × [ω e × R ] , is
caused by the angular velocity of the earth and changes as the location of the tool with respect to the
centre of the earth, R, changes. However, the change of R is small for the tool tracking applications
47
(maximum 4 m), and the multiplication of the square of angular velocity of the earth makes the
change in value insignificant. Therefore, the centripetal force due to the rotation of the earth can be
considered constant. Combining the centripetal force with the gravitational force results in different
local gravity values which can be written as:
f
g l = f g − ωe × f [ωe × R] .
(4-12)
The Coriolis force, 2 ⋅ ωe × f V , can be omitted when the velocity of the object is small because the
angular velocity of the earth is very small (7.3×10-5 rad/sec). Since the velocity of the tool is small
(usually less than 1 m/s) in the tool tracking application, the Coriolis term is much lower than the
noise level of the accelerometers; hence this term is ignored. Then, (4-11) can be simplified as
f
V& = f A+ f g l .
(4-13)
The specific force measurements are represented with respect to the tool frame because they are
measured with accelerometers. To change the specific force measurement terms from the tool frame
to the local fixed frame, a rotation matrix is required. Thus, (4-13) is rewritten as
f
V& = ft C ⋅t A+ f g l .
(4-14)
The rotation matrix from the tool frame to the local fixed frame can be determined from (4-10).
Since the Z-axis of the local fixed frame is chosen in the opposite direction of the local downwards,
the local gravity vector is expressed as
f
g l = [0 0
gl
]
(4-15)
The velocity and the position of the object can be calculated by integrating over time as follows:
f
V=
∫
f
V& ⋅ dt ,
(4-16)
f
P=
∫
f
V ⋅ dt ,
(4-17)
where P is the position of the object.
48
4.4 Expert System
4.4.1 Stationary State Identification
When the tool is stationary, its velocity is zero, but the calculated velocity using an IMU is usually
not zero. Thus, the calculated velocity should be corrected to zero when the tool is stationary. To
identify the stationary state, the IMU measurements are utilized. When an object is stationary, the
angular velocity is zero, the acceleration in each axis is constant, and the magnitude of the
acceleration is equal to the magnitude of the gravity vector.
However, the angular velocity
measurements are typically not zero because a gyro has several sources of error such as noise. The
acceleration measurements also contain noise, and the magnitudes of the acceleration measurements
due to gravity vary depending on the tilt angles because of the non-linearity. To identify the
stationary state from these inaccurate measurements, an expert system is incorporated by using
acceleration measurements and angular velocity measurements. Let the magnitude of gravity-free
acceleration (Acc) and the magnitude of angular velocity (Ang_vel) be expressed as
2
2
2
Acc = ( Ax + Ay + Az ) 0.5 − g l ,
2
2
2
Ang _ vel = (ω x + ω y + ω z ) 0.5 .
(4-14)
(4-15)
When the IMU is stationary, the accelerations of each axis should be constants. Thus, the
fluctuation of the acceleration measurements in each axis should be less than the maximum
acceleration noise. The acceleration fluctuation (Acc_fluc) is defined as
Acc _ fluc = (( Ax − Avg _ Ax ) 2 + ( Ay − Avg _ Ay ) 2 + ( Az − Avg _ Az ) 2 ) 0.5 ,
(4-16)
where the average accelerations are expressed as
n
Avg _ Ax =
∑ A (i) / n,
x
i =1
n
Avg _ A y =
∑A
y (i ) /
n,
i =1
n
Avg _ Az =
∑ A (i) / n,
z
i =1
49
(4-17)
where n is the number of acceleration samples. While the IMU is stationary, n increases to obtain
more steady average accelerations.
When the IMU is stationary, the gravity-free acceleration (Acc) should be lower than the maximum
error of the accelerometers which includes non-linearity error and the maximum noise. In addition,
the magnitude of the angular velocity (Ang_vel) should be lower than the maximum error of the gyro
which includes random walk and the maximum noise. Since the tool is manually moved, the IMU
measurements cannot maintain almost a constant acceleration in each axis and almost a zero angular
velocity for 1 second. Therefore, the IMU is considered stationary when the following conditions are
met:
Rule 4-1:
IF
Acc < ( Ax _ max_ error ) 2 + ( Ay _ max_ error ) 2 + ( Az _ max_ error ) 2 for the last 1 second data AND
Ang _ vel < (ω x _ max_ error ) 2 + (ω y _ max_ error ) 2 + (ω z _ max_ error ) 2 for the last 1 second data AND
Acc _ fluc < ( Ax _ max_ noise ) 2 + ( Ay _ max_ noise ) 2 + ( Az _ max_ noise ) 2 for the last 1 second data,
THEN stationary = 1 (IMU is stationary)
ELSE stationary = 0 (IMU is moving)
where Aaxis _ max_ error is the maximum acceleration error in each axis, ω axis _ max_ error is the maximum
angular velocity error in each axis, and Aaxis _ max_ noise is the maximum acceleration error in each axis.
When the IMU is stationary, the three accelerometers measure the tilt angles which are the angles
between the gravity vector and the IMU body frame axes. To correct the tilt angles, the relationship
between the tilt angles and the direction cosine matrix needs to be established. When the Z-axis of
the reference frame is chosen to be in the opposite direction of the gravity vector, the tilt angle
components of the direction cosine matrix from (4-1) are c xZ , c yZ , and c zZ .
The tilt angle
components can be derived by normalizing the three acceleration components. To reduce the effects
of the acceleration measurement noise, the average values are used to calculate the tilt angles instead
of the instantaneous accelerations. The tilt angle components in terms of the average acceleration in
each axis are
50
c xZ =
c yZ =
c zZ =
Avg _ Ax
2
2
2
( Avg _ Ax + Avg _ A y + Avg _ Az ) 0.5
Avg _ A y
2
2
2
( Avg _ Ax + Avg _ A y + Avg _ Az ) 0.5
Avg _ Az
2
2
2
( Avg _ Ax + Avg _ A y + Avg _ Az ) 0.5
,
,
(4-18)
.
From (4-10), the quaternion terms in the tilt angles are
2(q1 q 3 − q 0 q 2 ) = c xZ ,
(4-19)
2(q 2 q 3 + q 0 q1 ) = c yZ ,
(4-20)
2
2
2
2
q 0 − q1 − q 2 + q 3 = c zZ .
(4-21)
There are four unknowns and four equations (4-7) and (4-19) to (4-21). However, the equations
cannot be solved analytically because all four equations are in non-linear forms. To find the four
quaternion terms, one of them is fixed, and the other three terms are corrected. From (4-7) and (421),
2
2
2
2
1 − 2q1 − 2q 2 = 2q 0 + 2q 3 − 1 = c zZ .
(4-22)
When q0 is fixed, the corrected quaternion terms become
q0 _ cor _ 0 = q0 ,
(4-23)
q 3 _ cor _ 0 = sign(q 3 ) × ( c zZ + 1 − 2 × q 02 _ cor _ 0 / 2) 0.5 ,
(4-24)
q1 _ cor _ 0 =
q 2 _ cor _ 0 =
c xZ × q 3 _ cor _ 0 + c yZ × q 0 _ cor _ 0
2 × (q 02 _ cor _ 0 + q 32_ cor _ 0 )
,
− c xZ × q 0 _ cor _ 0 + c yZ × q 3 _ cor _ 0
2 × (q 02 _ cor _ 0 + q 32_ cor _ 0 )
(4-25)
,
(4-26)
where q0 _ cor _ 0 , q1 _ cor _ 0 , q2 _ cor _ 0 , and q3 _ cor _ 0 are the corrected quaternion terms. Eq. (4-25) and
(4-26) should be calculated after (4-23) and (4-24) are calculated because (4-25) and (4-26) use the
51
corrected quaternion terms. Since a quaternion has four components, four correction possibilities are
evaluated. The corrected terms are fed to (4-22) and the corrected terms that do not satisfy (4-22) are
discarded. From all the possible quaternion corrections, the correction with the minimum RMS error
is chosen as the best correction possibility. To prevent from further orientation drift, the angular
velocity is set to zero when the IMU is stationary.
The calculated non-zero velocity at the stationary state represents the velocity error. The position
of the IMU can be corrected by utilizing the velocity error at the stationary state [111], [117]. The
velocity error is partially caused by the integral of accelerometer error such as bias, gain error, nonlinearity, and noise. In addition, when the calculated orientation of the tool is not correct, the
acceleration calculation in (4-14) produces an error, which results in the accumulation of the velocity
error. When an IMU is accurately calibrated, it can be assumed that a large portion of the velocity
error is caused by orientation errors. Since the tool traveling time spans between fastening bolts are
short, it can be assumed that the velocity error is caused by a constant acceleration error which is
caused by constant orientation errors. Then, position estimation can be corrected by using the
following equation:
Paxis = Paxis − Vaxis ⋅ ∆t / 2
(4-27)
where Paxis is the current IMU position component in each axis, Vaxis is the calculated velocity error
in each axis at stationary state, and ∆t is the time span between the current time and the previous
velocity correction. Then, an expert rule is derived to correct the position and tilt angles as follows.
Rule 4-2: Position and orientation correction at stationary state.
IF
stationary = 1
THEN Paxis = Paxis − Vaxis ⋅ ∆t / 2 AND ω axis = 0 , AND Vaxis = 0 AND ω x = 0 , ω y = 0 , ω z = 0 AND
correct the quaternion terms using (4-23) to (4-26) AND n = n+1
ELSE n = 1
4.4.2 Initial Position Detection
The position and orientation of the tool can be corrected when the tool is placed in the tool holder.
Such error correction is possible because the position and orientation of the tool in the tool holder is
known. Therefore, the system should be able to identify if the tool is in the tool holder to correct the
52
position and orientation constantly.
In order to detect the initial position, the tool holder was
designed so that the tool is placed at a specific tilt angle (θ) as shown in Figure 4-2. The top of the
tool holder is shaped so that the bottom of the tool almost perfectly fits on it. However, there is a
small gap between the top of the holder and the tool bottom to ensure that the tool can easily be
placed on the holder. Therefore, it is possible for the tool to be rotated in the z-axis of the tool frame.
In order to calculate the rotation matrix from the tool frame to the local fixed frame ( ft C ), this
misalignment angle in the z-axis, φ, should be calculated as well.
The tilt angle, θ, and the
misalignment in z-axis, φ, have the following relationship with the measured acceleration components
and the gravity vector:
 Ax   cos ϕ
 A  = − sin ϕ
 y 
 Az   0
sin ϕ
cos ϕ
0
0 cos(−θ ) 0 − sin(−θ )  0 
⋅ 0  .
0 ⋅  0
1
0
 

1   sin(−θ ) 0 cos(−θ )   − g l 
(4-28)
Tool bit
110mm
110mm
A MEMS IMU
with a triaxial
magnetometer
Gravity
x
Y, y
Tool holder
θ
X
Figure 4-2: Fastening tool, the tool holder, and a sensor that consists of a triaxial magnetometer
and an IMU. The tool frame is labeled xyz and the fixed frame is labeled XYZ.
From (4-28), φ and θ are calculated with the accelerometer measurements as follows:
53
ϕ = arctan(− Avg _ A y / Avg _ Ax ),
θ = − arccos( Avg _ Az / g ).
(4-29)
The fixed frame (XYZ) is chosen so that the Z-axis is the opposite direction of the gravity vector,
and the x-axis of the tool is chosen so that it is parallel to the tool bit. Since (4-29) shows that Az
depends only on θ, Az can be used to identify the tool holder angle, θ. When Az − g ⋅ cosθ is less
than the threshold (the maximum error of the accelerometer) and the tool is stationary, the system
concludes that the tool is placed on the tool holder. Then, the quaternion terms of the rotation matrix
from the tool frame to the fixed frame can be corrected by using φ and θ as:
q0 2 + q12 − q2 2 − q3 2
2(q1q2 − q0 q3 )
2(q1q3 + q0 q2 ) 

2
2
2
2
q0 − q1 + q2 − q3
2(q2 q3 − q0 q1 ) 
 2(q1q2 + q0 q3 )
2
2
2
2
 2( q q − q q )
2(q2 q3 + q0 q1 ) q0 − q1 − q2 + q3 
1 3
0 2

.
 cos ϕ sin ϕ 0 cos(−θ ) 0 − sin(−θ ) 

= − sin ϕ cos ϕ 0 ⋅  0
1
0

 0
0
1  sin(−θ ) 0 cos(−θ ) 
(4-30)
There are ten equations {nine from (4-30) and one from (4-7)} and four unknowns. From (4-7) and
three equations that only have square terms, the square of each quaternion terms are identified. Then,
one of the four quaternion terms can be assumed either positive or negative, and the signs of the rest
three quaternion terms can be identified using the rest of the equations.
The expert rule to identify the initial position and to estimate the orientation is derived as follows.
Rule 4-3: Position and angle correction using the tool holder.
IF
stationary = 1 AND Az − g ⋅ cos θ < Threshold
THEN Paxis = 0 AND ϕ = arctan(− Avg _ A y / Avg _ Ax ) AND correct the quaternion terms by using
(4-7) and (4-30)
where Threshold is the maximum tilt angle measurement error.
4.4.3 Fastening Action Detection
The fastening action needs to be detected to identify the fastened bolt. However, the high
magnitude of vibration can cause high acceleration fluctuations. This may result in high velocity
54
error in discrete time domain. In order to reduce the magnitude of acceleration measurements due to
vibration, an IMU (3DM-GX2, Microstrain) is attached to the bottom of the tool as shown in Figure
4-2. This point has lower vibration than the top part of the tool because the gripping hand of an
operator acts as a damper. Due to the reduced magnitudes of vibration measurements, acceleration
measurements alone do not provide sufficient vibration to identify the fastening action. Thus, the
proposed system uses both acceleration and magnetic field measurements to detect the fastening
action.
Magnetic Field (Gauss)
Running
in the air
Fastening
action
1.6
1.4
1.2
1
0
2
4
0
2
4
6
8
10
12
6
8
10
12
2
Acceleration (m/s )
8
4
0
-4
-8
Time (sec)
Figure 4-3: Comparison study between the fastening action and running the tool in the air: (a)
magnitude of magnetic field and (b) magnitude of acceleration.
Figure 4-3 shows the magnetic field and the acceleration plots with 200 Hz sampling frequency
when the tool is running in the air (from 3 second to 5 second) and is fastening a bolt (from 7 second
to 9 second). The magnetic fields fluctuate in both scenarios, but the accelerations have high
55
frequency vibration only when the tool fastens a bolt. By using these two distinct measurement
signatures, the expert system can accurately detect a fastening action. From several test results, it has
become expert knowledge that the tool is fastening a bolt if (i) there are at least 3 peaks with at least
0.02 Gauss difference between the magnitude of the current magnetic field measurement (mag) and
the previous magnetic field measurement (pre_mag) in the last 0.1 second and (ii) there are at least
two magnitude of gravity-free acceleration (Acc) peaks that are greater than 1 m/s2 in the past 0.1
second. To identify this 0.1 second of time span, time count (fasten_time), which increases by one
every time step, is used. When the system detects the fastening action, the time count is changed to
0.05 second so that continuous fastening action detection is possible without reaching the 0.1 second
time limitation. The rules to identify the fastening action are described in Rule 4-3 to Rule 4-10.
Rule 4-3 to Rule 4-10: Fastening action detection
Rule 4-3:
IF
Acc > 1 m/s2 AND fasten_time ≥ (0.1 second × 200 Hz)
THEN fasten_time = 0 AND Acc_peak = 0 AND Mag_peak = 0 AND Acc_peak_high = 1 AND
Mag_peak_high = 0 AND Fasten_ Accel =0 AND Fasten_Mag = 0
Rule 4-4:
IF
Acc < 1 m/s2 AND Acc_peak_high = 1
THEN Acc_peak = Acc_peak + 1 AND Acc_peak_high = 0
Rule 4-5:
IF
Acc > 1 m/s2 AND Acc_peak_high = 0
THEN Acc_peak_high = 1
Rule 4-6:
IF
Acc_peak > 1 AND fasten _time < (0.1 second × 200 Hz)
THEN Fasten_Accel = 1
Rule 4-7:
IF
mag - pre_mag > 0.02 AND fasten_time < (0.1 second × 200 Hz)
THEN Mag_peak_high = 1
Rule 4-8:
IF
mag - pre_mag < -0.02 AND Mag_peak_high = 1
56
THEN Mag_peak = Mag_peak + 1 AND Mag_peak_high = 0
Rule 4-9:
IF
Mag_peak > 3 AND fasten _time < (0.1 second × 200 Hz)
THEN Fasten_Mag = 1
Rule 4-10:
IF
Fasten_Accel = 1 AND Fasten_Mag = 1 AND fasten_time < (0.1 second × 200 Hz)
THEN Fasten = 1 AND fasten_time = (0.05 second × 200 Hz) AND Acc_peak = 1 AND
Mag_peak = 3 AND Fasten_Accel = 0 AND Fasten_Mag = 0
where Acc_peak_high is an indicator that shows the acceleration is higher than 1 m/s2, Acc_peak is
the number of the gravity-free acceleration (Acc) peaks higher than 1 m/s2, Mag_peak_high is an
indicator that shows mag - pre_mag is greater than 0.02 Gauss, Mag_peak is the number of magnetic
field peaks more than 0.02 Gauss difference, Fasten_Accel is an indicator that the tool is fastening a
bolt based on acceleration measurements, Fasten_Mag is an indicator that the tool is fastening based
on magnetic field measurements, and Fasten is the fastening action indicator. The specific numbers
vary depending on the tool type and the location of the IMU.
The fastened bolt should be identified when the fastening action is detected. When a bolt is being
fastened, the position of the tool tip and the position of the fastened bolt head coincide, and the
fastened bolt angle is almost parallel to the tool bit angle. To identify the fastened bolt, the position
and orientation of the tool tip was compared with the position and orientation of each bolt. When the
gyros of 3DM-GX2 are calibrated, the orientation does not usually drift more than 3º/min. Since one
cycle of fastening process (fastening all the bolts in one workpiece) usually takes less than one
minute, the maximum orientation error of the system is 3º. On the other hand, position error can drift
over 500 mm in 10 second when a commercial grade IMU is used as shown in Figure 1-1. Thus, it is
the best to identify the fastened bolt using the orientation of the tool rather than the position of the
tool tip.
While a tool fastens a bolt, the orientation of the fastened bolt is parallel to the tool bit. Since the
x-axis of the body frame is parallel to the tool bit, the angle of the x-axis of the body frame is
compared with the angles of all the bolts in the workpiece to identify the fastened bolt. However, a
small gap between the tool bit and the bolt head can introduce a small orientation error. Therefore,
the fastened bolt may not be parallel to the threaded hole at the beginning of the fastening. In
57
addition, the orientation calculation drift due to the integration of angular velocity measurements
should be accounted for. By considering all the error margins, the expert system concludes that the
tool fastens one of the bolts which has less than a 15º error between the x-axis of the tool and the bolt
angles as follows.
c xX − Onx < 15°
c xY − Ony < 15° ,
(4-31)
c xZ − Ony < 15°
where Onx , Ony , and Onz are the orientation of the nth bolt with respect to the fixed frame.
If more than one bolt satisfies (4-31), the position information must be utilized to identify the
fastened bolt. While the tool fastens a bolt, the tool tip can be assumed stationary. In addition, the
angular velocity of the tool is almost zero because the tool bit fits in the bolt head and does not allow
the tool to rotate very much. Therefore, the linear velocity of the IMU is set to zero while the tool
fastens a bolt. Then, the position of the IMU can be corrected by using (4-27). After the position is
corrected, the position error between the corrected tool tip position and the nth bolt position ( Errn ) is
calculated as
 Px 
0.11  Pnx 


f 
Errn =  Py  + t C  0  −  Pny  ,
 Pz 
0.11  Pnz 
(4-32)
where Pnx , Pny , and Pnz are the position of the nth bolt. The bolt with the minimum position error is
chosen as the fastened bolt. Since the fastened bolt is identified, the position of the IMU ( Paxis ) can
be corrected by using the position of the fastened bolt ( Pf − axis ) and the rotation matrix as:
 Px   Pfx 
0.11
P  = P − f C  0  .
 y   fy  t 

 Pz   Pfz 
0.11
Then, the fastened bolt identification expert rule is as follows.
Rule 4-11: fastened bolt identification
IF
Fasten = 1
58
(4-33)
T
THEN Paxis = Paxis − Vaxis ⋅ t s / 2 , AND Fastened_bolt = bolt with minimum ( Errn ⋅ Errn ) 0.5 AND
Vaxis = 0 AND correct the IMU position using (4-33) AND Fasten = 0
ELSE fasten _time = fasten _time+1
where Fastened_bolt represents the fastened bolt number.
4.5 Experiments
The proposed fastened bolt tracking system was tested with a system consisting of a MEMS IMU
and a triaxial magnetometer shown in Figure 4-2. In order to obtain the true position and velocity, an
ultrasonic position sensor (CMS10, Zebris) is attached to the IMU. The tool fastens four bolts on an
aluminum tube shown in Figure 4-4. There are two sets of bolts with the same orientation, and these
bolts are 410 mm apart from each other. The tool fastened from Bolt 1 to Bolt 4 in sequence and
then placed back on the tool holder. The IMU sensors were sampled at 200 Hz.
410 mm
Bolt 1
Bolt 3
Bolt 2
Bolt 4
Figure 4-4: Workpiece with four bolts. Bolt 1 and Bolt 3 have the same orientation and Bolt 2
and Bolt 4 have the same orientation.
Figure 4-5 depicts the magnitude of magnetic field, the magnitude of acceleration, and the
identified fastened bolt number during the experiment. The figures indicate that the proposed system
identified the fastened bolt in the correct sequence.
59
(c)
Magnetic Field (Gauss)
2
Acceleration (m/s )
(b)
Fastened Bolt Number
(a)
1.6
1.4
1.2
1
0
5
10
15
20
25
30
0
5
10
15
20
25
30
0
5
10
15
20
25
30
20
10
0
-10
4
3
2
1
Time (S)
Figure 4-5: (a) Magnitude of magnetic field, (b) magnitude of acceleration, and (c) identified
fastened bolt number by the proposed method.
Figure 4-6 shows the velocity comparison among the true values, using the traditional navigation
system, and using the proposed method. Figure 4-6 illustrates that the true velocity almost matches
the velocity using the proposed method because the velocity error is corrected whenever the fastened
bolts are detected. In addition, the velocity is zeroed at 26 seconds when the tool was placed on the
tool holder. However, the velocity calculation by using the conventional navigation equations drifts
over time.
60
Velocity (m/s)
True
X
0.5
Proposed
Navigation
0
-0.5
0
5
10
15
20
25
30
0
5
10
15
20
25
30
0
5
10
15
20
25
30
Y
0.5
0
-0.5
Z
0.5
0
-0.5
Time (sec)
Figure 4-6: Velocity comparison in each axis among the true values measured with an
ultrasonic sensor, the calculated values using the conventional navigation equations, and the
calculated values using the proposed method.
Figure 4-7 displays the position comparison results between the true values measured with the
ultrasonic position sensor and the calculated values by using the proposed system. Figure 4-7 shows
the position comparison between the calculated values by using the proposed method and the true
values. When the fastening action is detected, the expert system corrects the position by using (4-27),
which is labeled “After position correction using ZUPT” (at 8.155 s) in Figure 4-7 (b). Then, by
comparing the orientation and the corrected position of the tool tip with the possible bolt positions
and orientations, the fastened bolt is identified. After the fastened bolt is identified, the expert system
calculates the position of the IMU using (4-33), which is labeled “After position correction using bolt
position” in Figure 4-7 (b). As a result, the position error is almost zero at 8.16 seconds as shown in
Figure 4-7 (b). The position error comparison before and after the position corrections using ZUPT
are shown in Table 4-1. This table shows that the fastened bolt can be identified more accurately by
using ZUPT.
61
Position (m)
(a)
0.3
True
Proposed
X
0
Y
-0.3
-0.6
0
5
10
15
20
25
30
0.8
0.6
0.4
0.2
0
-0.2
0
5
10
15
20
25
30
5
10
15
20
25
30
0.2
Z
0
-0.2
-0.4
0
Time (sec)
Position (m)
(b)
X
-0.1
-0.3
Before ZUPT
-0.5
8.14
8.145
After postion correction
using bolt position
After postion
correction using ZUPT
8.15
8.155
True
8.16
Calculated
8.165
8.17
Y
0.2
Before ZUPT
0
-0.2
8.14
After postion correction
using bolt position
After postion
correction using ZUPT
8.145
8.15
8.155
8.16
8.165
8.17
Z
0.05
0
-0.05
8.14
Before ZUPT
8.145
After postion correction
using bolt position
After postion
correction using ZUPT
8.15
8.155
8.16
8.165
8.17
Time (s)
Figure 4-7: Position comparison in each axis between the true values measured with an
ultrasonic position sensor and the calculated values using the proposed method (a) entire time
span and (b) magnified when Bolt 1 is fastened – before and after ZUPT, and after the position
correction using (4-33).
62
Table 4-1: Position Error using the proposed method before and after position correction.
Position error before position correction Position error after position correction
(mm)
using ZUPT (mm)
Bolt
X
Y
Z
Total
X
Y
Z
Total
1
-261
-198
-46
331
-52
14
6
54
2
370
-249
-125
463
100
72
-31
127
3
-138
-77
-153
220
-30
-6
69
75
4
34
128
-12
132
-66
-6
-22
70
4.6 Conclusion
This chapter presented a novel cost-effective fastened bolt tracking system that does not require a
line of sight. Such a system is intended for a workpiece that has different bolt orientations or long
distances between the bolts. The position and orientation of the tool are estimated by the use of an
IMU, and the fastening action is detected utilizing a triaxial accelerometer and a triaxial
magnetometer. The inaccuracies in position, velocity, and orientation are corrected by using an
expert system.
The expert system identifies the stationary state, initial position, and fastening action. When the
tool is stationary, the system corrects the velocity errors, tilt angle errors, and reduces the position
error. When the tool is on the tool holder, both the calculated position and orientation of the tool are
corrected. When the tool fastens a bolt, the system identifies the fastened bolt and corrects the
velocity and position error.
The experimental results indicate that the proposed system can identify the fastened bolt when the
time span between the fastening actions is short. When the bolts with the same orientation are closely
placed or the time span between the fastening actions is longer, a more accurate IMU must be used.
Therefore, the tradeoff between the required accuracy and the sensor cost should be decided
depending on the application.
63
Chapter 5
Fastening Tool Tracking System Using an IMU and a Position
Sensor with Kalman Filters and a Fuzzy Expert System
In the previous chapter, a fastened bolt tracking system using an IMU and a triaxial magnetometer
was presented. This system is low-cost and does not require a line of sight, but the time span between
the fastening bolts must be short or the bolt orientation must be different to correctly identify the
fastened bolts. In this chapter, a fastening tool tracking system consisting of an IMU and a position
sensor is presented. The presented system uses KFs and a fuzzy expert system to track the position of
the tool tip and to identify the fastened bolt.
5.1 Position Sensor Selection for Tool Tracking System
Selecting a position sensor depends on the required accuracy and the environment of the
application. Table 1-1 describes the accuracy, advantages and limitations of various position sensors.
The error of RF position sensors is too high for precision applications such as fastening tool tracking.
When the UWB position sensor is hybridized with an IMU, the accuracy might be improved enough
to be used for the fastening tool tracking system. However, the signal strength can be attenuated by
human tissue, and signals can be reflected by metal as discussed in subsection 2.1.3.
These
drawbacks make UWB position sensors unsuitable for a tool tracking system because a fastening tool
is always used in the proximity of an automotive metal part and an operator can easily block the lines
of sight between the emitter and the receivers. An electromagnetic position sensor is inappropriate
for an automotive manufacturing environment because magnetic fields are distorted by ferrous parts
as discussed in Chapter 1.
The ultrasonic position sensor requires three lines of sight and has performance issues related to
sound reflections and sound noise sensitivity. Since most automotive factories are subjected to sound
noise, an ultrasonic position sensor is not a reliable solution for a fastening tool tracking system. The
camera-based position estimation techniques require a complex calibration procedure and the
procedure can be time consuming. Since the whole assembly line can be stopped during calibration, a
time consuming calibration procedure is not desirable. Infrared position sensors are too expensive to
be installed for all fastening operations.
64
A string-encoder position sensor can track 3D position with a high accuracy and provides noisefree outputs. The string-encoder position sensor is a feasible option for a fastening tool tracking
system because fastening tools are often hung on a balancer in automotive factories as shown in
Figure 5-1. Since the position sensor acts as a balancer and support a tool, the end of the wire must
be connected to the centre of mass of the tool. This enables the position sensor to locate the centre of
mass of the tool instead of the tool tip. To determine the position of the tool tip with a string-encoder
position sensor, an IMU is used as an orientation sensor.
String balancer
Fastening tools
Figure 5-1: Two fastening tools are attached to a string balancer on the centre of mass of the
tools
5.2 Tool Tracking System Design with an IMU and an Encoder-Position Sensor
A string-encoder position sensor provides the location of the centre of mass of a tool, and an IMU
provides the orientation of the tool. With this sensor configuration, the position of the tool tip can be
calculated as
[T
x
] [
]
[
]
T
T y Tz = Px Py Pz + ft C Lx L y Lz .
(5-1)
The location from the tool tip to the centre of mass of the tool with respect to the tool frame, [Lx Ly Lz],
is fixed and can be predetermined. The location of the centre of mass of the tool is obtained by the
65
position sensor. The rotation matrix from the tool frame to the local fixed frame, ft C , is obtained by
integrating angular velocity measurements from the gyros of the IMU.
In order to compensate for the orientation drift, an intelligent system which utilizes KFs and a
fuzzy expert system is proposed. Figure 5-2 depicts the structure of the proposed system. An IMU
consists of accelerometers and gyros that have biases and gains. Thus, accelerometers and gyros
should be calibrated before use. The position sensor consists of encoders which always start from
zero whenever the sensor is turned on. Therefore, the position sensor should be initialized by locating
the sensor to a known position. A KF is used to estimate the orientation and the angular velocities of
the tool with gyros, and another KF is employed to estimate the positions and accelerations of the
centre of mass of the tool with an IMU and an encoder-based position sensor. Although two KFs can
be combined, they are separated because the position KF requires orientation information, and using
two KFs is computationally cheaper. The workpiece information provides the locations and the
orientations of all bolts. All the information is processed with an expert system to identify the
fastened bolt and to correct the orientation error. When the expert system detects a fastening action,
the fastened bolt is identified. Then, the complete 3D orientation of the tool can be determined by
using the location and the orientation of the fastened bolt and the location of the position sensor; thus,
the orientation error can be corrected. When the tool is stationary, the tilt angles of the tool are
corrected. The fuzzy expert system outputs the orientation and position of the centre of mass of the
tool. Then, the location of the tool tip is estimated by using (5-1).
IMU
Gyroscopes
Calibration
Accelerometers
Calibration
Position sensor
Initialization
Orientation
Kalman
filter
Angular velocity
Orientation
Orientation
Centre of mass
Position
location of tool
Kalman
Acceleration
filter
Time derivative
Workpiece
information
Velocity
Location and orientation of bolts
Figure 5-2: Overview of the fastening tool tracking system
66
Fastening
action detection
Bolt
identification
Steady state
detection
Orientation
correction
Fuzzy
Expert System
Location
of tool tip
5.2.1 Orientation Calculation Using Kalman Filtering
Gyros measure angular velocities with respect to the inertial frame whose origin is at the centre of
the earth and its axes are stationary with respect to the fixed stars [116]. However, the tool tracking
system is used in a small area, and the local fixed frame is attached to a stationary building which
rotates 360°/day with the earth. Thus, the position and orientation changes due to the rotation of the
earth must be discarded. In order to discard the angular velocity of the earth, gyros should be
calibrated so that the angular velocity is zero when the IMU is stationary with respect to the local
fixed frame. Even when the rotation rate of the earth is not accounted for the calibration, the earth
rotation rate does not make any difference because the rotation matrix is corrected often and the
-5
rotation rate of the earth is only 7.27×10 rad/sec, which is often smaller than the noise level of a
MEMS gyro. Therefore, the rotation rate of the earth can be discarded. Figure 5-3 shows the tool
frame and the local fixed frame which is attached to the lab building. The Z-axis of the local fixed
frame is chosen in the opposite direction of the local gravity vector, and the z-axis of the tool frame is
chosen so that it is parallel to the bolt socket axis, which is parallel to the fastened bolt.
Z
X
Y
y
Local Fixed
Frame
Tool
z Frame
x
Bolt socket
Tool tip
Figure 5-3: Local fixed frame and tool frame: the Z-axis of the local fixed frame is the opposite
direction of the gravity vector and the z-axis of the tool frame is along the bolt socket axis.
To define the orientation of the tool with respect to the reference frame, the quaternion
representation is used. To utilize the KF, the quaternion equation needs to be written in the form of
(2-5). The differential equation of quaternion q with respect to time has the following matrix form:
67
 q&0 
 q0
 q& 

 1  = 1  q1
q& 2  2 q2
 

 q&3 
 q3
− q1
− q2
q0
q3
− q2
− q3
q0
q1
− q3   0 
q2  ω x 
⋅
.
− q1  ω y 
  
q0  ω z 
(5-2)
Since there are four quaternion states and three angular velocity measurements, the state of
orientation, x ori , becomes
[
xori = q0
q1
q2
ω y ω z ]T .
q3 ω x
(5-3)
From (5-2) and (5-3), the system matrix of orientation in the form of (2-5) becomes
Φ ori
2
0

0
1
= 0
2
0

0
0

0 0 0
− q1 ⋅ t
2 0 0
0 2 0
q0 ⋅ t
q3 ⋅ t
0 0 2 − q2 ⋅ t
0 0 0
2
0 0 0
0 0 0
0
0
− q 2 ⋅ t − q3 ⋅ t 
− q3 ⋅ t q2 ⋅ t 
q0 ⋅ t − q1 ⋅ t 

q1 ⋅ t
q0 ⋅ t  .
0
0 

2
0 
0
2 
(5-4)
The system input matrix of orientation, Γori , is a zero matrix because the tool is rotated by a human
operator. Also, since the quaternion states are estimated from the angular velocities, the process noise
of the system is
[
bori = 0 0 0 0 bang
bang
bang
]
T
,
(5-5)
where bang is the process noise of the angular velocity of the tool.
The angular velocities are measured by gyros, but there is no sensor that measures quaternion
components. Since the gyros are calibrated, the measurement matrix for the orientation in the form of
(2-6) becomes
H ori = [03 x 4
I 3x3 ] .
(5-6)
To satisfy (4-7), all the quaternion components should be normalized after they are calculated.
The states of a KF converge on meaningful estimations when the states are observable. The
observability matrix (OM) is calculated as
68
 H 
 H ⋅F 
,
OM = 


M

m −1 
H ⋅ F 
(5-7)
where m is the dimension of the state vector x. If the observability matrix has a rank of m, then the
states are completely observable.
For the orientation KF, the rank of the observability matrix is three. In other words, only three
angular velocities are observable, and the four quaternion terms may not converge to the correct
values. Therefore, a quaternion term correction method needs to be implemented. The details of this
correction method will be discussed in Section 5.3.
5.2.2 Position Kalman Filter Using an IMU and a Position Sensor
For the position estimating system, six measurements are available: three position components in
the local fixed frame from the position sensor and three acceleration components in the tool frame
from the IMU. From (4-14) and (4-15), the three acceleration measurements in the tool frame can be
expressed in the local fixed frame in the form of (2-5) as
V&x = c xX ⋅ Ax + c yX ⋅ A y + c zX ⋅ Az
V&y = c xY ⋅ Ax + c yY ⋅ A y + c zY ⋅ Az
.
(5-8)
V&z = c xZ ⋅ Ax + c yZ ⋅ A y + c zZ ⋅ Az + g l
By applying a constant acceleration motion model, the equations of velocity in the local fixed frame
are
P&x = V&x ⋅ t + Vx
P&y = V&y ⋅ t + V y .
(5-9)
P&z = V&z ⋅ t + Vz
According to (5-8) and (5-9), the states of the position KF, x pos , are defined as follows:
[
x pos = Px Vx
Ax
Py V y
69
Ay
Pz Vz
]
T
Az .
(5-10)
The position and velocity components are represented in the local fixed frame, and the
acceleration components are represented in the tool frame. From (5-8) and (5-9), the system dynamic
matrix of object position in discrete time, Φ pos , becomes
Φ pos
1

0
0

0
= 0

0

0
0

0
t c xX ⋅ t 2 / 2 0 0 c yX ⋅ t 2 / 2 0 0 c zX ⋅ t 2 / 2

1
c xX ⋅ t
0 0
c yX ⋅ t
0 0
c zX ⋅ t 

0
1
0 0
0
0 0
0

0 c xY ⋅ t 2 / 2 1 t c yY ⋅ t 2 / 2 0 0 c zY ⋅ t 2 / 2 
0
c xY ⋅ t
0 1
c yY ⋅ t
0 0
c zY ⋅ t  .

0
0
0 0
1
0 0
0


2
2
2
0 c xZ ⋅ t / 2 0 t c yZ ⋅ t / 2 1 t c zZ ⋅ t / 2 
0
c xZ ⋅ t
0 0
c yZ ⋅ t
0 1
c zZ ⋅ t 

0
0
0 0
0
0 0
1

(5-11)
The gravitational force is treated as a deterministic input. Since the Z-axis is parallel to the gravity
vector, the velocity and the position changes due to the gravity vector should be compensated for in
the Z-axis. Thus, the system input matrix of position is
[
Γpos ⋅ u pos = 0 0 0 0 0 0
gl ⋅ t 2 / 2
]
T
gl ⋅ t 0 .
(5-12)
Since the position and the velocity states are estimated with three accelerations, the process noise is
wk −1, pos = qaccel ⋅ [0 0 1 0 0 1 0 0 1] ,
T
(5-13)
where qaccel is the process noise of the tool acceleration.
The acceleration of each axis is measured by three accelerometers, and the position components
are measured by a position sensor. Since both accelerometers and the position sensor are calibrated
and initialized, the measurement matrix for the position estimation becomes
H pos
1
0

0
=
0
0

0
0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0
.
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1
70
(5-14)
The rank of the observability matrix of the position KF is nine. Therefore, all nine states are
observable.
This position KF utilizes the orientation of the tool which is estimated from the
orientation KF. These filters reduce the measurement noise of accelerations and angular velocities.
In the next section, the dynamic status of the tool such as stationary state is identified using the
filtered measurements.
5.3 Fuzzy Expert System for Tool Tracking System
5.3.1 Tilt Angle Correction
The system in the previous chapter identifies the stationary state by using the IMU measurements.
The presented system in this chapter identifies the stationary state of the tool by using three sets of
states: (i) angular velocity, (ii) acceleration fluctuation, and (iii) linear velocity. The estimations of
accelerations and angular velocities from the KFs are used instead of the direct measurements from
the IMU because the estimated values from the KFs have less noise. However, the derivatives of the
encoder-based position sensor outputs are chosen for the linear velocity calculation because they are
zero when the tool is stationary.
Although the KFs are used to estimate acceleration and angular velocity states, only a portion of
the noise is removed, and the linear acceleration and the angular velocity estimations still suffer from
nonlinearity and random walk. To identify the stationary state from these measurements, a fuzzy
expert system is proposed. The rules are developed based on the properties of the sensors in using
linguistic terms. If the fluctuation of the acceleration estimation is high or the angular velocity
estimation is high, the tool is not stationary even though the linear velocity from the position sensor is
zero. This case occurs when the tool is hung on the wire of the position sensor, and the tool rotates
about an axis: i.e., the wire of the position sensor. The fuzzy sets of the expert system inputs are
shown in Figure 5-4 a) and b). The magnitude of angular velocity (Ang_vel) is calculated with
angular velocity estimations after the KF by using (4-15) instead of using angular velocity
measurements.
Also, the acceleration fluctuation (Acc_fluc) is calculated with the acceleration
estimations after the KF using (4-16). Figure 5-4 c) exhibits the dynamic states of the tool. Table 5-1
lists the fuzzy rules to identify the stationary state of the tool.
71
b)
medium
0.8
0.6
0.4
0.2
0
c)
high
0
0.02
0.04
0.06
0.08
medium
high
0.8
0.6
0.4
0.2
0
0.1
low
1
Membership Degree
low
1
Membership Degree
Membership Degree
a)
0
0.04
0.08
0.12
0.16
0.2
2
Ang_vel (rad/s)
static
1
quasi-static moving
0.8
0.6
0.4
0.2
0
0
0.2
0.4
0.6
0.8
1
Dynamic State
Acc_fluc (m/s )
Figure 5-4: Membership functions of the fuzzy expert system for the tilt angle correction
algorithm.
Table 5-1: Fuzzy rules to identify the stationary state of the tool.
Fuzzy rules
Ang_vel
Acc_fluc Dynamic state
1
Low
Low
Static
2
Low
Med
Quasi-static
3
Med
Low
Quasi-static
4
Others
Moving
When the derivative of the position sensor is zero for 0.1 second and the dynamic state is less than
0.5 for 0.1 second, the tool is considered stationary. The 0.1 second period is chosen because it is not
realistic for an operator to move a tool so that the dynamic state is less than 0.5 and the derivative of
the encoder-based position sensor output is zero for 0.1 second unless the tool is stationary. Also, the
0.1 second period is used to calculate the average accelerations in each axis. According to the above
assumption, the fuzzy expert rules for the stationary state identification of a tool are shown in Table
5-2 (Rule 5-1 and Rule 5-2). When the system concludes that the tool is stationary, the tilt angles are
corrected by using the method in Subsection 4.4.2. To prevent a further orientation drift, the angular
velocity is set to zero when the IMU is stationary (Rule 5-3 in Table 5-2).
5.3.2 Fastening Action Detection
To identify the fastening action, the acceleration signature is studied and distinguished from other
possible movements that the fastening tool can experience (Figure 5-5). The identification of the
fastening action by using the frequency domain is difficult because the frequency contents of the
72
fastening action depend on many variables such as materials and the shape of the workpiece. Instead,
the time domain is used for the analysis of the fastening action.
The acceleration signature of the fastening action (Figure 5-5 a)) denotes a very high vibration and
high magnitude of acceleration. Figure 5-5 b) shows the base excitation when the tool is free-running
in the air without fastening a bolt. The acceleration of the base excitation has a high frequency of
vibration, but it does not have a high magnitude of acceleration. Figure 5-5 c) depicts the situation
when the tool is manually shaken by a person. It displays high magnitudes of acceleration but does
not exhibit a high frequency of vibration. Figure 5-5 d) shows normal movements when the tool is
moved around. This movement does not have a high frequency of vibration or high peaks of
acceleration.
This study demonstrates that fastening action can be identified with acceleration
frequency contents and the magnitudes of acceleration.
Let the magnitude of acceleration
measurement (Acc_mea) be
2
2
2
Acc _ mea = ( Ax + Ay + Az ) 0.5 .
(5-15)
If at least three peaks satisfy Acc_mea >20 m/sec2 within a 0.1 second interval, the expert system
concludes that the tool is fastening a bolt. In order to measure the 0.1 second interval, a time variable,
fastening_period, is initialized when the accelerometers first detect Acc_mea >20 m/sec2, and
increases as the time elapses. If the system does not detect three peaks that satisfy Acc_mea >20
m/sec2 within 0.1 second interval, it is initialized again when the next peak is detected. The rules for
the fastening action detection are shown in Table 5-2 (Rule 5-4 to Rule 5-7). The specific values of
the expert system conditions such as the peak values of Acc_mea may vary depending on the shapes
and the materials of the bolt and the workpiece.
a)
b)
Acceleration (m/s2)
50
c)
50
d)
50
50
40
40
40
40
30
30
30
30
20
20
20
20
10
10
10
10
0
0
2
4
0
0
2
0
4
0
2
4
0
0
2
4
Time (sec)
Figure 5-5: Possible acceleration measurement signatures of the fastening tool used in different
scenarios: a) fastening action, b) base excitation, c) hand vibration, and d) normal movements.
73
5.3.3 Fastened Bolt Identification
When the fastening action is detected, the tool tracking system must identify the fastened bolt.
The fastened bolt identification process is summarized in Figure 5-6. There are three scenarios for
the bolt identification:
1.
The position sensor alone identifies the fastened bolt.
2.
When Scenario 1 fails, the IMU information is added to verify the fastened bolt.
3.
When Scenario 2 fails, the system assumes that the bolt with the highest possibility of being
fastened is fastened and waits for the next fastening action. When the system identifies the
next fastened bolt, it identifies the previously fastened bolt.
When the system successfully identified the fastened bolt, it corrects the orientation error and outputs
the fastened bolt number.
Only one bolt
Scenario 1
is within
Orientation
position sensor
correction
Yes
error margin
Fastening
action
detection
No
Output
fastened bolt
number
Scenario 2
Fastened bolt
Use fuzzy
is identified
expert system
to identify the
fastened bolt
Fastened bolt is Scenario 3
not identified
Waiting for
fastening
action
detection
Fastening
action
detection
Correct orientation Fastened bolt
is identified
and identify
previously fastened
bolt
Yes
Only one bolt
No
is within
position
sensor error
Use fuzzy
expert system
to identify
fastened bolt
Fastened bolt is not identified
Figure 5-6: Fastened bolt identification process.
When the tool fastens a bolt, the location of the tool tip coincides with one of the bolts (Figure 57).
Then, the distance between the position sensor and one of the bolt position should be
2
2
2
( Lx + L y + Lz ) 0.5 . However, the position sensor has error, and the location of the bolt also has
error due to manufacturing uncertainties. When the tool fastens a bolt, the position sensor error with
respect to the nth bolt is
74
2
2
2
PSEn = ((n x − Px ) 2 + (n y − Py ) 2 + (n z − Pz ) 2 ) 0.5 − ( Lx + L y + Lz ) 0.5 .
(5-16)
PSEn includes the error of the position sensor as well as the manufacturing error of the workpiece.
Thus, when the tool fastens a bolt, the following inequality holds for at least one bolt:
PSEn ≤ MPSE + MMU .
(5-17)
If there is only one bolt that satisfies (5-17), the system concludes that the tool fastens that bolt. In
this application, the maximum position sensor error is ±7mm.
Position of nth
bolt/tool tip
[nx, ny, nz]
[Lx, Ly, Lz]
Position sensor
location
[Px, Py, Pz]
0
Tool
Position sensor error
envelope
Figure 5-7: Position sensor error envelope while a tool fastens a bolt. The position of the tool tip
coincides with one of the bolts.
It is possible that more than one bolt satisfies (5-17). In this case, the tool tip position calculation
by using (5-1) is utilized to identify the fastened bolt. However, identifying the fastened bolt does not
simply correspond to finding the closest bolt from the calculated tool tip position because the
orientation error is unknown. To identify the fastened bolt in the presence of orientation uncertainties,
a fuzzy expert system is utilized.
Two inputs are employed to identify the fastened bolt: the calculated tool tip position error and the
run-time which is the duration from the previous complete orientation correction to the current time
excluding the stationary state period. As the calculated tool tip position error with respect to the bolt
75
decrease, the probability of fastening the bolt gets higher. The calculated tool tip position error is
represented as
PEn = ((Tx − n x ) 2 + (Ty − n y ) 2 + (Tz − n z ) 2 ) 0.5 .
(5-18)
As the run-time increases, the reliability of the calculated tool tip position decreases. Both
position error and run-time are modeled, and the membership degrees with respect to the nth bolt are
shown in Figure 5-8. The two antecedent conditions are aggregated with the min operation [118],
[119] to obtain the implication on the consequence membership function. Figure 5-8 c) shows the
membership degree output for the nth bolt.
The bolt with the highest membership degree of
Bolt_Output_1 is chosen as the fastened bolt because it has the highest probability of being fastened.
When the membership degree of the calculated tool tip position error is lower than that of run-time,
the closest bolt from the calculated tool tip position is the fastened bolt as shown in Figure 5-9.
b)
0.8
0.6
0.4
0.2
0
c)
0
50
100
1
0.8
0.6
0.4
0.2
0
150
Membership Degree
1
Membership Degree
Membership Degree
a)
0
50
Position Error (mm)
100
150
200
Run-Time (sec)
1
0.8
0.6
0.4
0.2
0
n
Bolt_Output_1
Figure 5-8: Membership degree functions of a) calculated tool tip position error, b) run-time,
and c) the output for Bolt n.
Calculated position
of tool tip
Membership
Degree
Position Error
membership curve
Bolt 1
Bolt 2
Figure 5-9: Fastened bolt identification when position error is the determining factor.
76
However, if run-time becomes the determining factor, multiple bolts will have the highest
membership degree in Bolt_Output_1 fuzzy set in Figure 5-8 (c). If all the bolts are fastened except
for one, the system concludes that the unfastened bolt is now being fastened. If more than one bolt is
not fastened, the system cannot identify the fastened bolt. Then, the system uses another fuzzy expert
system to identify the bolt with the highest probability of being fastened using the position error and
position sensor error as shown in Figure 5-10. The two antecedent conditions are multiplied to obtain
the membership degree of Bolt_Output_2 for the nth bolt and the bolt with a higher membership
degree is assumed fastened. In this scenario, the fastening system also examines the next fastened
bolt to ensure that the output of the fuzzy expert system is the correct fastened bolt. When the next
fastening action is detected, the system identifies the fastened bolt and finds the path from the
fastened bolt to the previous fastened bolt with respect to the previous possible bolt position and
orientation.
By comparing the distances and the orientations between the possible previously
fastened bolts and the currently fastened bolt, the system can identify the previously fastened bolt.
The expert rules for the fastened bolt identification phase are shown in Table 5-2 (Rule 5-8 to Rule 511).
a)
0.8
0.6
0.4
0.2
0
50
100
Membership Degree
1
0
c)
Membership Degree
Membership Degree
b)
1
0.8
0.6
0.4
0.2
150
Position Error (mm)
0
0
2
4
6
Position Sensor Error (mm)
1
0.8
0.6
0.4
0.2
0
n
Bolt_Output_2
Figure 5-10: Membership degree functions of a) calculated tool tip position error, b) position
sensor error, and c) output for Bolt n.
5.3.4 Orientation Correction
When a tool fastens a bolt, the orientation of the bolt and the vector that connects the position
sensor and the tool tip position, which coincide with the bolt position, are known. Since two vectors
are known, the complete orientation of the tool can be determined. The tool frame is chosen so that
the z-axis is parallel to the socket, which is almost parallel to the fastened bolt when the tool fastens a
bolt. This axis gives the z-axis vector of the direction cosine matrix: c zX , c zY , and c zZ . If the tool
77
frame is selected so that Ly is zero, the location of the tool tip with respect to the centre of mass in the
tool frame becomes [Lx 0 Lz]. From (5-1), the x-axis vector of the direction cosine matrix becomes
c xX =
c xY =
c xZ =
n x − c zX L z − Px
Lx '
n y − c zY L z − Py
Lx '
,
(5-19)
n z − c zZ L z − Pz
Lx '
where L x ' = (n x − c zX L z − Px ) 2 + (n y − c zY L z − Py ) 2 + (n z − c zZ L z − Pz ) 2 . Lx ' is used to normalize
the x-axis vector of the direction cosine matrix instead of Lx . This is attributed to the fact that Lx
may not match Lx ' due to position sensor error ( PSEn ). The y-axis vector of the rotation matrix can
be calculated from the cross-product of the two vectors of the rotation matrix. When all three vectors
of the direction cosine matrix are determined, three tool frame vectors of the rotation matrix must be
normalized. Then, four quaternion terms can be determined from the rotation matrix. The four
quaternion terms should be normalized to satisfy (4-7).
Table 5-2: Rules for the fastening tool tracking system in linguistic terms.
Stationary State Identification
Rule 5-1
IF the derivative of the position sensor output is zero for the last 0.1 second AND dynamic
state < 0.5 for the last 0.1 second, THEN update the average accelerations AND the tool is
stationary.
Rule 5-2
IF the tool was stationary in the previous time step AND the derivative of the position sensor
output is zero AND dynamic state < 0.5, THEN update the average accelerations AND the
tool is stationary.
Rule 5-3
IF the tool is stationary, THEN ω x = ω y = ω z = 0 AND correct the quaternion terms using (423) to (4-26)
Fastening Action Detection
Rule 5-4
IF Acc_mea > 20 m/s2 AND fastening_period > 0.1 second THEN fastening_period = 0 AND
peak = 0 AND peak_high = 1.
Rule 5-5
IF Acc_mea > 20 m/s2, THEN peak_high = 1
78
Rule 5-6
IF Acc_mea < 20 m/s2 AND peak_high = 1 THEN peak = peak +1 AND peak_high = 0
Rule 5-7
IF peak = 3 AND fastening_period < 0.1 second, THEN the tool is fastening a bolt.
Fastened Bolt Identification
Rule 5-8
IF the tool is fastening a bolt AND only one bolt that satisfies PSEn ≤ MPS + MME , THEN
the tool is fastening the nth bolt.
Rule 5-9
IF the tool is fastening a bolt AND there are more than one bolt that satisfy
PSE n ≤ MPSE + MME , THEN the bolt with the maximum Bolt_Output_1 membership value
is being fastened.
Rule 5-10
IF the tool is fastening a bolt AND there is more than one bolt that has the highest
Bolt_Output_1 membership value AND all the bolts were fastened except one bolt, THEN
the tool is fastening the unfastened bolt.
Rule 5-11
IF the tool is fastening a bolt AND more than one bolt have the highest Bolt_Output_1
membership value AND more than one bolt is not fastened before, THEN assume that the
bolt with the maximum Bolt_Output_2 membership value is being fastened.
IF the next fastening action is detected, THEN identify the fastened bolt AND identify the
previously fastened bolt.
Rule 5-12
IF the fastened bolt is identified THEN correct the quaternion terms.
5.4 Experiments
The fastening tool tracking system is attached to a right angle tool and examined on the testbed
shown in Figure 5-11. Eight bolts are placed on the testbed, and their positions are shown in Table 53. When the tool was positioned on Bolt 1, 4, 5, 6, and 7, the bolts were fastened, but when the tool
was on Bolt 2, 3, and 8, the tool ran in the air right above the bolts to test if the system can
differentiate the two different scenarios. The tool started from the initial position and moved for 204
seconds before it was returned to the initial position. The tool moved from Bolt 1 to Bolt 8 in
sequence, and after Bolt 6 was fastened, the tool was left stationary for 80 seconds to test the
stationary state identification fuzzy expert system. The test results of tracking the tool tip are
illustrated in Figure 5-12.
79
Start and finish
position
1
3
2
5
7
6
8
4
Figure 5-11: Testbed for the lab experiment.
Table 5-3: Bolt positions shown in Figure 5-11.
Bolt number
Bolt position
Bolt number
Bolt position
Bolt 1
(-0.4. -0.34, 0)
Bolt 5
(-0.05, -0.24, 0)
Bolt 2
(-0.4. -0.44, 0)
Bolt 6
(-0.05, -0.34, 0)
Bolt 3
(-0.3. -0.34, 0)
Bolt 7
(0.05, -0.24, 0)
Bolt 4
(-0.3. -0.44, 0)
Bolt 8
(0.05, -0.34, 0)
b)
300
Y (mm)
100
start
finish
-100
5
-300
-500
-900
-700
-500
1
3
2
4
-300
6
300
100
start
7
Y (mm)
a)
-100
finish
-300
5
1
3
8
2
-100
-500
-900
100
X (mm)
-700
-500
-300
6
7
8
4
-100
100
X (mm)
Figure 5-12: Tool tracking results a) with the intelligent system and b) without the intelligent
system.
80
Figure 5-12 a) shows the trajectory of the calculated tool tip position with the intelligent system
which includes the KFs and the fuzzy expert system. Figure 5-12 b) depicts the trajectory of the
calculated position of the tool tip without the intelligent system. When the system detects the
fastening action, a square mark with * symbol is added to the fastened bolt position. Figure 5-12 a)
shows that Bolt 1, 4, 5, 6, and 7 are marked; indicating that the system successfully identified the
fastened bolt. The start and finish points are almost identical when the intelligent system is used.
However, Figure 5-12 b) reveals that the finish point is different from the start point when the tool tip
position is calculated without the expert system. The calculated tool tip position errors with and
without the intelligent system are summarized in Table 5-4 (with respect to the fastened bolt positions
and the finish position). The total position errors of Table 5-4 are calculated by using (5-18). The
position error without the intelligent system increases over time because the orientation error drifts
over time.
However, the proposed intelligent system reduces the position error because the
orientation error is corrected when the tool fastens a bolt and when the tool is held stationary.
Table 5-4: Position error comparison between with and without the intelligent system.
Tool
Time
Position error with the intelligent Position error without the intelligent
position
(s)
system (mm)
system (mm)
X-axis
Y-axis
Z-axis
Total
X-axis
Y-axis
Z-axis
Total
Bolt 1
19
-2
0
6
6
-2
1
6
6
Bolt 4
43
-12
-6
-6
15
-10
-4
9
14
Bolt 5
85
-2
-8
2
8
-9
-5
37
38
Bolt 6
89
-4
-2
2
5
-8
-7
38
39
Bolt 7
184
3
-6
7
10
8
-58
28
65
Finish
204
-1
-6
0
6
-27
-33
83
93
5.5 Conclusion
This chapter presented an intelligent tool tracking system that utilizes a hybrid sensor
configuration consisting of an IMU and a position sensor. KFs were developed to estimate the
orientation and the position of the tool more accurately. The outputs of the sensors are related to
identify if the tool is stationary or fastening a bolt. When the tool is stationary, the system corrects
81
the tilt angles. When the tool fastens a bolt, the system identifies the fastened bolt and corrects the
orientation error. The intelligent system was validated through experiments.
The fastening tool tracking system was tested with a manufacturing assembly example in a
laboratory setting. The position error of the tool tip increases as the time of operation elapses when
the intelligent system is not used because the orientation error increases over time. However, by
utilizing the intelligent system, the fastened bolts are correctly identified, and the position error is
reduced.
82
Chapter 6
Fastening Tool Tracking System Using a Combined
Kalman/Particle Filter
This chapter proposes a novel position/orientation estimation technique. The proposed method
combines a KF and a PF to hybridize one IMU and one position sensor. The KF is used to estimate
the position, and the PF is used to estimate the orientation. Then, this method is revised to apply to
the fastening tool tracking system to identify the fastened bolts. The test results of the proposed
system in this chapter are compared with those of the KF-based intelligent system that was introduced
in Chapter 5.
6.1 Overview of the Position/Orientation Tracking System Combining the KF
and the PF
The proposed position/orientation estimation method is presented in Figure 6-1. The proposed
method calculates the position and orientation states from IMU measurements and measures the
position using a position sensor. An expert system is developed to correct the angular velocities
according to the measurements from the IMU and the position sensor. By using the corrected angular
velocities ( ω x , ω y , and ω z ), the rotation matrix from the body frame to the fixed frame is estimated.
The rotation matrix of the ith particle from the body frame to the fixed frame at time t k ( bf Cki ) is
represented by using quaternions as:
 c1ik
 i
f i
b Ck = c 4 k
c7 ik

c 2ik
c5ik
c8ik
c3ik 

c6 ik 
c9ik 
q 0i 2 + q1i 2 − q 2i 2 − q3i 2
k
k
k
 k
=  2(q1ik q 2ik + q0 ik q3ik )

i
i
i
i
 2(q1k q3k − q 0 k q 2 k )
2(q1ik q 2 ik − q 0ik q3ik )
2
2
2
q0 ik − q1ik + q 2ik − q3ik
2(q 2ik q3ik + q 0ik q1ik )
 , (6-1)

2(q 2ik q3ik − q0ik q1ik ) 
2
2
2
2
q0 ik − q1ik − q 2ik + q3ik 

2(q1ik q3ik + q0 ik q 2ik )
2
where c#ik is the component of the direction cosine matrix of the ith particle at time t k , and q0ik , q1ik ,
q2ik , and q3ik are the four components of a unit quaternion. Since the orientation is calculated using
83
i
i
a unit quaternion, each orientation particle consists of four states ( xPF
,k = [ q 0 k
q1ik
q 2ik
q3ik ] )
that satisfy
2
2
2
2
q 0ik + q1ik + q 2ik + q3ik = 1 .
Accelerometer
Faculty
Calibrated
IMU
Gyroscopes
Ax , Ay , Az
(6-2)
Filtering Technique
Position
Estimation
q 0 i , q1i , q 2 i q3i
Expert System
Position,
Velocity
Evaluate all
particles
Orientation
particle filter
ω x ,ω y ,ω z
q 0 i , q1i , q 2 i q3i Delay
Position sensor
Position
and
orientation
Position
Kalman filter
Px , Py , Pz
Figure 6-1: Outline of the proposed method.
The quaternion components of each particle can be computed from the angular velocity
measurements using:
 q0 ik +1 
 2
 i 

 q1k +1  = 1 ω x ,k ⋅ t
q 2 ik +1  2 ω y ,k ⋅ t
 i 

ω z ,k ⋅ t
 q3k +1 
− ω x,k ⋅ t
− ω y ,k ⋅ t
2
ω z ,k ⋅ t
− ω z ,k ⋅ t
ω y ,k ⋅ t
2
− ω z ,k ⋅ t   q0 ik 


− ω y ,k ⋅ t   q1ik 
.
⋅
ω x ,k ⋅ t  q 2 ik 

 
2   q3ik 
− ω x ,k ⋅ t
(6-3)
The corresponding velocity and position of each orientation particle are estimated using a KF. From
(4-14) and (4-17), the motion tracking equations of particle i for short distance navigation can be
written as:
f
V& i = bf C i ⋅b A+ f g l ,
(6-4)
P& i = ∫ f V i dt ,
(6-5)
f
where f V i represents the velocity of the ith particle in the fixed frame. Eq. (6-4) reveals that the
acceleration measurements in the body frame ( b A = [ Ax
Ay
Az ]T ), which includes the gravity
vector, are multiplied by the rotation matrix to calculate the acceleration in the fixed frame. Thus,
when the rotation matrix is inaccurate, the misalignment between the true body frame and the
84
calculated body frame leads to an acceleration error in (6-4), which consequently results in velocity
and position errors. In other words, a high orientation error can result in a high position error. The
proposed filter evaluates orientation particles by comparing the position calculation of each particle
with the position estimate of each particle from the KF. The particles with lower position difference
between the two are assigned higher weights.
6.2 Position Kalman Filter
A KF is used to estimate the position of each particle in the proposed method because the state
space model of an IMU is linear and the noise distribution of many position sensors can be considered
zero-mean Gaussian. To utilize the KF, the motion equations in (6-4) and (6-5) must be rewritten in
the form of (2-5). The state vector of the KF of the ith particle at time t k is defined as
[
i
i
x KF
, k = Pkx , k
Vkxi ,k
i
Akx
,k
Pkyi ,k
Vkyi ,k
i
Aky
,k
Pkzi ,k
Vkzi ,k
Akzi ,k
],
T
(6-6)
where Pki−axis ,k , Vki−axis ,k , Aki −axis ,k are the position, velocity, and acceleration of the ith particle in each
axis using the KF respectively. Then, the system transition matrix of the ith particle, Φ ik , is
1

0
0

0
Φ ik = 0

0

0
0

0
t
1
0
0
0
0
0
0
0
0 0 c 2ik ⋅ t 2 / 2 0 0 c3ik ⋅ t 2 / 2 

c 2ik ⋅ t
c3ik ⋅ t 
0 0
0 0

1
0 0
0
0 0
0

i
2
i
2
i
2
c 4 k ⋅ t / 2 1 t c5 k ⋅ t / 2 0 0 c 6 k ⋅ t / 2
c 4ik ⋅ t
c5ik ⋅ t
c6ik ⋅ t  .
0 1
0 0

0
0 0
1
0 0
0


i
2
i
2
i
2
c7 k ⋅ t / 2 0 t c8k ⋅ t / 2 1 t c9 k ⋅ t / 2
c7 ik ⋅ t
0 0
c8ik ⋅ t
0 1
c9ik ⋅ t 

0
0 0
0
0 0
1

c1ik ⋅ t 2 / 2
c1ik ⋅ t
(6-7)
Gravity is treated as a deterministic input. When the direction of the Z-axis of the fixed frame is
chosen opposite to the local downwards, the velocity and the position changes due to gravity
measurements must be compensated for along this direction. Since the system input matrix is not
related to the orientation particle, it is written as:
[
Γk ⋅ u k −1 = 0 0 0 0 0 0
85
gl ⋅ t 2 / 2
]
T
gl ⋅ t 0 .
(6-8)
Since the magnitude of the local gravity vector can be assumed constant for short distance navigation,
the system input matrix is assumed to be constant.
The acceleration and the position components are directly measured using calibrated
accelerometers and a position sensor. Then, the measurement matrix is:
1
0

0
Hk = 
0
0

0
0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0
.
0 0 0 0 1 0 0 0
0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1
(6-9)
6.3 Orientation Filtering Technique
The proposed method uses the PF to estimate the orientation of an object. The weight of each
particle is determined by using the position calculation of each orientation particle and the position
measurements as the most probable value. In such an approach, however, the differences between the
position measurements and the position calculation of each orientation state can be a result of not
only the orientation state but also sensor errors such as noise. Then, the particles with higher
orientation errors can have higher likelihoods of being in the correct orientation. In other words, the
position measurements at time t k are not accurate enough to approximate the true posterior of
orientation.
To overcome this problem, the summation of the position differences for a period of time ( ∆Ts ,
where subscript s is the sth orientation iteration, s = 1, 2, …) is selected to determine the weights of
the particles instead of the instantaneous position differences at time t k . In addition, instead of using
the direct position measurements, the KF position estimations of each particle, which incorporates the
position measurements, are used to reduce the effect of the measurement noise. Then, the likelihood
is calculated based on the accumulated position difference between the estimations and the calculated
values of the ith particle ( APEsi ) as
APE si =
M s ⋅s
∑ {( P
i
px ,k
− Pkxi ,k ) 2 + ( Ppyi ,k − Pkyi ,k ) 2 + ( Ppzi ,k − Pkzi ,k ) 2 } ,
k =( s −1)⋅M s +1
86
(6-10)
where M s = ∆Ts / t , and Ppi −axis ,k are the position states of the ith orientation particle at time tk . The
lower APEsi of the given particle signifies the higher likelihood of being the correct orientation. For
every ∆Ts period elapses, the weight of each particle is recalculated according to APE si values.
Then, the PF must be modified to use APE si instead of the direct position measurements. The
posterior approximation up to time t s is
N
p( x0:s | APE1i:s ) ≈
∑ w δ (x
i
k
0:s
− x0i :s ) .
(6-11)
t =1
Then, the posterior up to time t s is expressed as
p( x0:s | APE1i:s ) ∝ p ( APEsi | xs ) ⋅ p( xs | xs −1 ) ⋅ p ( x0:s −1 | APE1i:s −1 ) ,
(6-12)
and the normalized weight of the ith particle at the sth orientation iteration becomes
wsi ∝
p ( APE si | x si ) ⋅ p ( x si | x si −1 )
r ( x si
|
x si −1 ,
APE si )
⋅ wsi −1 .
(6-13)
By resampling and choosing the importance density from prior, p( xs | xs −1 ) and the fact that
wsi −1 = 1 / N , the normalized weight has the following relationship:
i
wsi ∝ p ( APE si | x PF
,s ) .
(6-14)
The weight of the orientation particle is calculated based on APEsi and the most probable value of
APE si .
argmin( APEsi ) is chosen as the most probable value because the orientation with the
minimum accumulated position error has the highest likelihood of being the correct orientation. Then,
the normalized weight is calculated as:
 − ( APEsi − arg min( APE si )) 2 
,
wsi ∝ exp
i 2

2
×
(
σ
(
APE
))
s


(6-15)
where σ ( APE si ) is the standard deviation of APEsi . Since 3σ ( APEsi ) contains 99.7% of the values
of APEsi , it is valid to assume that the mean of APEsi less argmin( APEsi ) is 3σ ( APEsi ) . After
87
resampling, the quaternion terms must be normalized to satisfy (6-2). The summary of the proposed
particle filtering technique is described in Table 6-1.
Table 6-1: Summary of the proposed particle filtering technique.
If k = 1, Draw x0i from p( x0 ) and w0i = 1 / N
// Initialization
i
Calculate xPF
,k
// Prediction
Calculate APEsi according to (6-10)
// Likelihood
IF remain (k / M s ) = 0
// Update
i
x si = x PF
,k
// Predicted states
 − ( PEsi − arg min( APEsi )) 2 
 // Weight calculation
Calculate w *is = exp

2 × (σ ( APEsi )) 2


N
wsi = w *is /
∑ w*
i
s
// Normalize weights
i =1
Draw xsi based on wsi
// Draw the next particles (Resampling)
wsi = 1 / N
// Reset weights
i
i
x PF
,k = x s
End IF
Note that ∆Ts should be chosen based on the sensor accuracy.
If ∆Ts is too short, the
accumulated position difference may not be large enough to help identify the best orientation particle.
When ∆Ts is too long, the orientation error may become large prior to the application of the
orientation evaluation. This, in turn, impacts the orientation estimation accuracy. In addition, this
algorithm requires more particles to represent the PDF to cover a wider range of possible orientation
angles leading a higher computational cost.
6.3.1 Angular Velocity Correction Using an Expert System
The expert system is used in two cases: (i) estimating the initial gyro biases and (ii) detecting the
stationary state. In Chapter 4, an accelerometer calibration method with a simple procedure was
introduced. Although it provides a high accuracy, the procedure takes about thirty seconds and the
88
gyro calibration technique in Appendix B takes about 30 seconds. When the proposed KF-PF
combined method in this chapter is used, simpler calibration procedure can be employed.
Both gyros and accelerometers suffer from biases and gain errors primarily caused by temperature
drift. Thus, many commercial IMUs are factory calibrated and have a temperature drift compensation
feature to reduce the effects of biases and gain errors, as well as misalignment and non-linearity.
Also, acceleration sensitivities of the gyros are compensated for.
Even when an IMU has a
temperature compensation feature, the gains and biases slightly differ whenever the sensor is
switched on as discussed in Chapter 2. However, the effects of the accelerometer gain and bias errors
due to power-on can be insignificant in this application because the gain and bias differences due to
power-on are usually very small and a position sensor is used to correct the velocity and position
estimation errors.
The gain difference of a gyro due to power-on is small, and the orientation error caused by the
gain error due to power-on is not significantly high. However, the gyro bias error can cause a
significant orientation error because gyro bias leads to continuous orientation drift over time and there
are no orientation measurements to correct the orientation. As the angular velocity bias increases, the
proposed filter will require a higher process noise and more particles to compensate for the
orientation uncertainty in order to maintain the orientation accuracy. Since the bias variation due to
power-on is also very small, a slight increment in the numbers of particles and process noise can
remove the effect of the small bias error. However, this can still result in higher computational
complexity. Since factory calibrated IMUs have the acceleration sensitivity compensation for gyros,
the gyro biases can be calculated by simply leaving the object stationary. Then, the mean value for
the stationary state of each gyro becomes the bias. Since this procedure is very simple and fast, the
gyro bias is calibrated in the proposed system. The IMU is left stationary for 1 s, and the mean values
are subtracted from gyro measurements to compensate for the biases. The rules for the gyro initial
bias compensation are defined as follows:
Rule 6-1: IF time ≤ 1 second THEN
ω _ sumaxis = ω _ sumaxis + ω _ measaxis AND ω axis = 0
Rule 6-2: IF time = 1 second THEN
ω _ biasaxis = ω _ sumaxis × t
Rule 6-3: IF time > 1 second THEN
89
ω axis = ω _ measaxis − ω _ biasaxis
where ω _ sumaxis is the angular velocity summation of each axis, ω _ biasaxis is the calculated
angular velocity bias of each axis, and ω _ meas axis is the angular velocity measurement of each axis.
It is expected to achieve higher position and orientation accuracy when the accelerometers and
gyros are calibrated more accurately. However, when the proposed KF-PF approach is used with a
factory calibrated IMU, the effect of calibration is not significant. Although the gyro biases are not
significantly high, they are calculated and removed because the procedure is simple and takes only 1
second, and bias removal reduces the orientation error.
Another role of the expert system is to identify the stationary state.
Bayesian estimation
techniques are suitable for estimating dynamic states, but an alternative approach is required for
estimating the states that do not change. Thus, when the object is stationary, the system should output
the previous states instead of estimating the current state. When the object is identified as being
stationary, the angular velocity is changed to zero. Even when the angular velocity is zero, the yaw
angle can still drift during the resampling step. This is due to the fact that the yaw angle drift does
not affect the position calculation when the object is stationary. Therefore, while the object is
stationary, the resampling step is suspended to ensure the current orientation state is the same as the
i
i
previous orientation state ( xPF
,k = x PF ,k −1 ). To suspend the resampling step, ∆Ts is increased by one
sample time whenever the object is identified as being stationary ( ∆Ts = ∆Ts + t ).
The stationary state is identified by using the similar approach to that in Chapter 4. Since a
position sensor is available, the position measurement fluctuation ( Pos _ flucaxis ) of each axis is
defined as
Pos _ flucaxis = Paxix − Avg _ Paxis ,
(6-16)
where the average position of each axis ( Avg _ Paxis ) are expressed as
n
Avg _ Paxis =
∑P
axis (
j ) / w, .
(6-17)
j =1
When the object is stationary, the magnitude of the angular velocity of each axis should be lower than
the maximum angular velocity error of the gyro, the acceleration fluctuation ( Acc _ flucaxis ) is
smaller than the maximum acceleration noise from the accelerometer, and the fluctuation of the
90
position measurement is less than the maximum error of the position sensor. Then, the expert rule for
the stationary state is
Rule 6-4:
IF ωaxis ≤ ω axis _ max_ error for the last t stationary seconds
AND Acc _ fluc axis < Aaxis _ max_ noise for the last t stationary seconds AND Pos _ flucaxis < Paxis _ max_ error
for the last t stationary seconds,
THEN ω axis = 0 , and ∆Ts = ∆Ts + t
where Paxis _ max_ error is the maximum position error in each axis. The t stationary period should be
chosen depending on the application.
6.3.2 Initial Orientation Estimation
In order to use the PF, the initial PDF of the orientation states need to be identified. Two extreme
cases can be considered: (i) when the orientation state is almost completely known and (ii) when the
orientation of the object is completely unknown. When the initial orientation of the object is known
with a high level of certainty, the initial PDF can be represented as a normal distribution with a very
small covariance.
However, in many circumstances, the initial orientation is completely unknown. Then, two
different approaches are possible. First, quaternion states are drawn from a uniform distribution. In
this case, a high initial process noise covariance is selected because the uncertainty of the orientation
state is initially high. It is assumed that the orientation converges to the correct value as time elapses.
Then, the uncertainty also reduces over time. In order to incorporate this knowledge, the process
noise covariance monotonically reduces to the final value where the orientation is known with a high
degree of certainty.
The second approach is to utilize the knowledge that the object is stationary for the first 1 second.
Then, the average acceleration measurements for the first 1 second can be used to calculate the tilt
angles so that the uncertainties in roll and pitch angles ( θ x and θ y respectively) are reduced. The
relationship between roll and pitch angles and the acceleration measurements due to gravity can be
expressed as [116]:
91
θ x = arctan( Avg _ Ay _ i / Avg _ Az _ i ),

− Avg _ Ax _ i
θ y = arcsin
 ( Avg _ Ax _ i + Avg _ Ay _ i + Avg _ Az _ i )

2
2
2 0.5

,


(6-18)
where Avg _ Aaxis _ i is the initial average acceleration of each axis for the first one second using (417).
Although the roll and pitch angles can be determined from (6-16), the uncertainty in the yaw angle
remains an issue. If any information about the yaw angle is available, it can be used to construct the
initial PDF. However, if no information is available about the initial state of the yaw angle, a uniform
distribution can be used to represent the PDF of the yaw angle. If 20 particles are used, the particles
are placed 18° (360°/20 particles) apart in the yaw angle domain. Then, the initial quaternion states
by using these angles are defined as follows [116]:
q 00i = cos(
q1i0 = sin(
q 2i0
θx
2
θx
2
= cos(
q30i = cos(
) ⋅ cos(
) ⋅ cos(
θx
θx
2
2
θy
) ⋅ sin(
2
θy
2
) ⋅ cos(
θy
) ⋅ cos(
2
θy
2
) ⋅ cos(
θ zi
2
θ zi
2
) ⋅ cos(
) ⋅ sin(
θ zi
2
θ zi
2
) + sin(
) − cos(
2
θx
) + sin(
) + sin(
θx
2
θx
2
θx
2
) ⋅ sin(
) ⋅ sin(
θy
θy
) ⋅ cos(
) ⋅ sin(
2
2
θy
2
θy
2
) ⋅ sin(
) ⋅ sin(
θ zi
2
θ zi
2
) ⋅ sin(
) ⋅ cos(
θ zi
2
θ zi
2
)
)
,
(6-19)
)
)
where θ zi is the ith yaw angle at 1 second.
Similar to the first approach, a high initial process noise covariance is selected and monotonically
reduced until it reaches the final value. However, since this algorithm already has the roll and pitch
angle information, the initial process noise covariance should be selected lower than the level chosen
for the first approach of case (ii).
6.4 Experiments
6.4.1 Preliminary Experiment
In order to test the novel PF-KF method described in this chapter, the presented theory is first
tested in a lab environment with a factory calibrated IMU, and then applied to the tool tracking
experiment.
92
The proposed method was experimentally tested by using a hybrid system that consists of one
factory calibrated IMU (3DM-GX2, Microstrain) and one position sensor (Optotrak, NDI). Optotrak
can locate infrared light emitting diode (IRED) position markers with submillimeter error [52], [53].
One IRED position marker was attached to the centre of the IMU. To check the orientation accuracy
of the proposed method, the true orientation was calculated from four IRED position markers placed
280 mm apart from each other. The configuration of the sensors is depicted in Figure 6-2. Optotrak
requires lines of sight between the IRED position markers and the cameras. Since an IRED position
marker has a limited signal emitting angle, three sets of Optotrak cameras were used to track the
position of the markers.
280 mm
Four position markers
for the true orientation
measurements
280 mm
Hybrid system consists
of one IMU and one
position marker
Figure 6-2: The proposed hybrid system and the true orientation measurement system.
In this experiment, the sensors shown in Figure 6-2 were moved manually in random 3D motions
for 8 minutes and returned to the original position and orientation. The sensor was kept stationary for
the first 1 second to remove the gyro bias and for the last 10 second to check if the expert system
could detect the stationary state. The true position and orientation measurements using Optotrak are
illustrated in Figure 6-3.
Both the position sensor and the IMU were sampled at 100 Hz (t = 0.01 s). Since it is unlikely to
move an object and satisfy Rule 6-4 for 1 second when the object is manually moved, t stationary is set to
1 second. The proposed method evaluates the weights of the particles every 1 second interval while
the tool is moving ( ∆T1:s = 1 s). When the system detects stationary state, ∆T can be extended as
93
described in Rule 6-4. Four different analyses were conducted for the same test: (i) orientation error
comparison between the EKF and the proposed filter, (ii) orientation error comparison of the
proposed filters with different numbers of particles, (iii) orientation error comparison of the proposed
filters with different numbers of particles when the initial condition is completely unknown, and (iv)
position and orientation error comparisons when noise is added to the position measurements.
(a)
(b)
30
Y (m)
Roll (°)
1
0.5
0
-0.5
0
120
240
360
0
-30
-60
480
1.5
60
1
30
0.5
0
-0.5
0
120
240
360
60
1
30
0
-0.5
0
120
240
360
480
Time (s)
120
240
360
480
0
120
240
360
480
0
120
240
360
480
0
1.5
0.5
0
-30
-60
480
Yaw (°)
Z (m)
True Orientation
60
Pitch (°)
X (m)
True Position
1.5
0
-30
-60
Time (s)
Figure 6-3: True (a) position and (b) orientation measurements using Optotrak.
First, the orientation errors obtained with both an EKF and the proposed filter with 20 particles are
compared in Figure 6-4. The expert system described in Section 6.3 was applied to both the EKF and
the proposed filter to correct the angular velocity. Figure 6-4 indicates that the orientation errors
using the EKF increase over time, but the errors using the proposed filter are significantly reduced
and do not grow over time. The roll and pitch angle errors of the proposed method have lower
magnitudes than the yaw angle error because the roll and pitch angles are associated with the gravity
vector when the acceleration of the object is calculated. As a result, the orientation particles with
higher roll and pitch angle errors have lower weights and die out faster. To analyze the orientation
estimation accuracy better, the RMS errors of the rotation matrices are compared in Figure 6-5. The
results show that the rotation matrix error using an EKF increases over time. When the proposed
method is applied, the rotation matrix error is significantly reduced and the error does not increase
94
over time. The errors of the last 10 seconds in Figure 6-4 and Figure 6-5 did not change because the
angular velocity was reduced to zero and the resampling was suspended when the object was
stationary.
(a)
(b)
Roll ( °)
EKF
16
12
8
4
Pitch ( °)
0
Yaw ( °)
0
120
240
360
480
16
12
8
4
0
0
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
16
12
8
4
0
120
240
360
480
16
12
8
4
0
Proposed Method
16
12
8
4
0
16
12
8
4
0
120
240
360
480
0
Time (s)
Time (s)
Figure 6-4: Orientation errors using (a) EKF and (b) the proposed method with 20 particles.
Rotation Matrix Error
(a)
(b)
EKF
Proposed Method
0.12
0.12
0.1
0.1
0.08
0.08
0.06
0.06
0.04
0.04
0.02
0.02
0
0
120
240
360
480
0
0
Time (s)
120
240
360
480
Time (s)
Figure 6-5: RMS rotation matrix error using (a) EKF and (b) the proposed method with 20
particles.
For the second analysis, the orientation errors were calculated using the proposed method with 5,
20, and 80 particles. Figure 6-6 and Figure 6-7 show the corresponding orientation errors in each
case. When the number of particles is increased from 5 to 20, the orientation error is significantly
95
reduced. However, when the number of particle is increased from 20 to 80, the graphs show no
significant improvement. This experiment shows that increasing the number of particles does not
proportionally improve the orientation estimation accuracy.
Yaw ( °)
Pitch ( °)
Roll ( °)
(a)
(b)
5 Particles
(c)
20 Particles
80 Particles
4
4
4
3
3
3
2
2
2
1
1
1
0
0
0
120
240
360
480
0
120
240
360
480
0
4
4
4
3
3
3
2
2
2
1
1
1
0
0
0
120
240
360
480
0
120
240
360
480
0
4
4
4
3
3
3
2
2
2
1
1
1
0
0
0
120
240
360
480
0
120
Time (s)
240
360
480
0
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
Time (s)
Time (s)
Figure 6-6: Euler angle errors using the proposed method when the initial orientation is known
(a) 5 particles, (b) 20 particles, and (c) 80 particles.
Rotation Matrix Error
(a)
(b)
5 Particles
(c)
20 Particles
80 Particles
0.04
0.04
0.04
0.03
0.03
0.03
0.02
0.02
0.02
0.01
0.01
0.01
0
0
120
240
360
480
0
0
120
Time (s)
240
360
Time (s)
480
0
0
120
240
360
480
Time (s)
Figure 6-7: RMS rotation matrix errors using the proposed method when the initial orientation
is known (a) 5 particles, (b) 20 particles, and (c) 80 particles.
For the third analysis, the initial orientation was assumed completely unknown, and the orientation
of the object was estimated using the proposed filters with 5, 20, and 80 particles. The initial
orientation is distributed using (6-19) at 1 second. In addition, ∆T1 is chosen 3 seconds instead of 1
96
second so that the majority of particles with small weights die out in the first orientation iteration.
When the initial orientation is unknown, high initial process noise is selected and then decreased
gradually over time to its final value used in the previous two analyses.
The initial process noise and the settling time are reduced as the number of particles increase.
When 5 particles are used, the initial process noise is 40 times higher than the final value. Then, the
process noise converges monotonically to the final value in 390 second. When 20 particles are used,
the initial process noise is 16 times higher than its final value and converges to this value in 150
second. When 80 particles are used, the initial process noise is four times higher than its final value.
This process noise converges to the final value in 30 second.
Roll ( °)
(a)
Pitch ( °)
(c)
20 Particles
80 Particles
90
90
60
60
60
30
30
30
0
0
120
240
360
480
0
0
120
240
360
480
0
90
90
90
60
60
60
30
30
30
0
0
120
240
360
480
0
0
120
240
360
480
0
180
180
180
120
120
120
60
60
60
0
0
Yaw ( °)
Magnified
Yaw ( °)
(b)
5 Particles
90
0
120
240
360
480
0
120
240
360
480
0
8
8
8
6
6
6
4
4
4
2
2
2
0
0
0
120
240
360
Time (s)
480
0
120
240
Time (s)
360
480
0
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
Time (s)
Figure 6-8: Euler angle errors when the initial orientation is unknown: (a) 5 particles, (b) 20
particles, and (c) 80 particles.
Figure 6-8 shows the absolute values of Euler angle errors (the actual roll and pitch angle errors
vary from -90° to 90°, and the yaw angle error varies from -180° to 180°) for the previous three
scenarios when the initial orientation is completely unknown. Figure 6-8 (a) depicts the orientation
error using the proposed filter with 5 particles. Since only 5 particles are used to span the entire yaw
97
angle range, this approximation cannot represent the true PDF very well. As a result, the particle with
a high yaw angle error (120°) has the highest weight at the beginning and slowly converges to the
correct orientation. Figure 6-8 (a) shows that the orientation converges to the correct values as time
elapses even though the initial values were far from the correct orientation. When the yaw angle is
around 90°, the roll and pitch angle errors become unstable in Figure 6-8 (a), which is a well-known
Euler angle problem. The last row of Figure 6-8 shows the magnified yaw angle errors. The plots in
the last rows of Figure 6-8 depict that as time elapses, the yaw angle errors almost match their
counterparts in Figure 6-6. The last row of Figure 6-8 (b), the proposed method with 20 particles,
illustrates that the correct orientation was found at 70 second. However, the yaw angle error started
increasing significantly because the number of particles is small and the process noise is high. Since
the process noise is designed to reduce monotonically to the final value in 150 s, the yaw angle error
does not significantly increase after 120 second. Figure 6-8 demonstrates that the proposed filter
finds the correct orientation faster as the number of particles increase.
For a close up view of Figure 6-8 (c), the first 15 seconds of the proposed method with 80 particles
are shown in Figure 6-9. Initially, the orientation particles were randomly drawn from a uniform
distribution. At 1 second, the roll and pitch angels were estimated by using (6-18). At this instant,
the roll and pitch angle errors are close to zero, and the yaw angle is represented by 80 particles that
are uniformly distributed using (6-19). Around 6 seconds, after 3 seconds of movement period
excluding the stationary state, the orientation particle with about 130° difference from the correct yaw
angle is chosen as the output because it has the highest weight. At 7 seconds, however, the yaw angle
error is considerably reduced indicating that the filter contains a set of particles with low orientation
errors. In other words, since 80 particles are used, many particles with low yaw angle error survived
during the resampling at 6 seconds and were chosen as output at 7 seconds.
The same experiment was repeated with 20 particles without using (6-18) to estimate the initial
roll and pitch angles. The initial process noise is 26 times higher than the final value and is allowed
to converge to the final value in 250 seconds. Figure 6-10 displays the experimental results. As
expected, this method requires a longer time to converge to the correct orientation due to the
uncertainties in the roll and pitch angles.
98
Roll (°)
90
60
30
0
0
3
6
9
12
15
0
3
6
9
12
15
0
3
6
9
12
15
Pitch (°)
90
60
30
0
Yaw (°)
180
120
60
0
Time(s)
Figure 6-9: First 15 seconds of Figure 6-8 (c), the orientation error with 80 particles when the
initial orientation is unknown.
Roll ( °)
(a)
Pitch ( °)
(a)
Magnified
6
60
4
30
2
0
0
120
240
360
480
0
90
6
60
4
30
2
0
0
Yaw ( °)
(b)
Full range
90
120
240
360
480
0
180
6
120
4
60
2
0
0
120
240
360
480
0
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
Time (s)
Time (s)
Figure 6-10: Euler angle errors using the proposed method with 20 particles when the initial
orientation is unknown and accelerometer measurements are not used to estimate the roll and
pitch angles, (a) full range and (b) range from 0° to 6°.
99
Roll ( °)
(a)
Pitch ( °)
(c)
20 Particles
80 Particles
6
6
4
4
4
2
2
2
0
0
120
240
360
480
0
0
120
240
360
480
0
6
6
6
4
4
4
2
2
2
0
Yaw ( °)
(b)
5 Particles
6
0
120
240
360
480
0
0
120
240
360
480
0
6
6
6
4
4
4
2
2
2
0
0
120
240
Time (s)
360
480
0
0
120
240
360
480
0
0
120
240
360
480
0
120
240
360
480
0
120
240
360
480
Time (s)
Time (s)
Figure 6-11: Euler angle errors of the proposed method with (a) 5 particles, (b) 20 particles, and
(c) 80 particles when Gaussian noise is added to the position measurements.
Lastly, the effect of the position measurement noise on the orientation error is studied. To
investigate the effects of the position measurement noise on the orientation and position accuracies,
the same experiment was repeated with Gaussian noise added to the Optotrak position measurements.
In this analysis, the position measurements are treated as the true position because Optotrak has only
submillimeter error.
Figure 6-11 shows the orientation error using the proposed method with
additional position noise, and Fig 6-12 shows the corresponding position error. The plots in Figure 611 display lower orientation errors than those obtained using the EKF in Figure 6-4 (a). This is
particularly evident for the roll and pitch angles. This result reveals that the proposed method has a
good performance even in the presence of additional Gaussian position noise. As expected, Figure 66 and Figure 6-11 illustrate that the position measurement noise tends to reduce the orientation
accuracy. Figure 6-11 also shows that the orientation accuracy increases as the number of particle
increases. However, increasing the number of particles increases the computational cost as shown in
Table 6-2. The proposed method was calculated by using Intel Core™2 Duo Processor E8400 with 3
Gb memory. Matlab was chosen to analyze the data off-line. Based on the results in Table 6-2, the
100
proposed method with 80 particles is not feasible for on-line application with the current system
because the processing time exceeds the experiment duration, 480 seconds. However, the processing
time can be reduced by using faster processor or more dedicated solvers such as those that utilize C or
Java development platform. It can also be concluded that the number of particles should be chosen to
achieve a tradeoff between computational power and estimation accuracy.
Table 6-2: Number of particles and their processing time.
Number of particles
5 particles
20 particles
80 particles
Processing time
43 s
142 s
521 s
Figure 6-12 shows the position RMS error using three different approaches for position
estimation: (i) no filter (added position noise), (ii) an EKF, and (iii) the proposed methods with three
different particle sizes. The position estimations of both the EKF and the proposed methods show
improved results over the case when no filter was used. The proposed methods in all three cases yield
lower position errors than the EKF estimation method. Figure 6-12 also suggests that the proposed
method provides higher position accuracy when the number of particles is increased.
Figure 6-11 relates that the position measurement noise affects the orientation accuracy, and the
orientation error decreases as the number of particles increases. In the next experiment, a random
position noise is added to the position measurements instead of a zero-mean Gaussian noise. The
orientation errors of this test with different numbers of particles were compared in Figure 6-13, and
the position error is compared in Figure 6-14.
Position Error (m)
(a)
No Filter
(b)
EKF
(c)
5 Particles
(d)
20 Particles
(e)
80 Particles
0.1
0.1
0.1
0.1
0.1
0.08
0.08
0.08
0.08
0.08
0.06
0.06
0.06
0.06
0.06
0.04
0.04
0.04
0.04
0.04
0.02
0.02
0.02
0.02
0.02
0
0
120 240 360 480
Time (s)
0
0
120 240 360 480
Time (s)
0
0
120 240 360 480
Time (s)
0
0
120 240 360 480
Time (s)
0
0
120 240 360 480
Time (s)
Figure 6-12: RMS position error when Gaussian noise is added to the position measurements:
(a) added Gaussian noise using no filter (b) using an EKF (c) using the proposed filter with 5
particles, (d) using the proposed filter with 20 particles, and (e) using the proposed filter with 80
particles.
101
The position error in Figure 6-14 is a little bit worse than the position error in Figure 6-12. This is
most likely because the position measurement error is not Gaussian.
The orientation error
comparison between Figure 6-13 and Figure 6-11 also shows that the orientation error is slightly less
accurate when the position measurement has a uniform noise distribution. Since the proposed method
utilizes the KF to estimate position, the accuracy can decrease when the position measurement noise
is non-Gaussian. However, both the position and orientation estimations are improved when the
proposed PF-KF combination is used.
Figure 6-13: Euler angle error of the proposed method with (a) 5 particles, (b) 20 particles, and
(c) 80 particles when random noise is added to position measurements.
102
Figure 6-14: RMS position error when random noise is added to the position measurements: (a)
added random noise using no filter (b) using the proposed filter with 5 particles, (c) using the
proposed filter with 20 particles, and (d) using the proposed filter with 80 particles.
In the next experiment, the position sensor mimics the encoder-based position sensor that was
used for the fastened bolt tracking system. The encoder-based position sensor does not have noise
and have maximum 0.007 m of error. Also, the position error of the encoder-based position sensor
gradually increases or decreases. This error characteristic of the encoder-based position sensor is
represented with added sinusoidal position signal with the attitude of 0.007 m. The sinusoidal
position error of each position axis has different frequencies (0.1, 0.15, and 0.3 rad/sec). Although
the sinusoidal position error is a source of error, it is not measurement noise. Thus, the measurement
noise covariance is the same as the noise covariance of Optotrak. Figure 6-15 reflects the orientation
error comparison with different numbers of particles. The graphs show that they are almost the same
as Figure 6-6, where the additional measurement error was not added. The orientation error is
improved when the particle size is increased from 5 to 20, but there is no significant difference
between the orientation errors with 20 particles and with 80 particles.
103
Figure 6-15: Euler angle error with the proposed method with (a) 5 particles, (b) 20 particles,
and (c) 80 particles when sinusoidal position errors with an attitude of 0.007 m is added to the
position measurements.
6.5 PF-KF-Based Tool Tracking System
6.5.1 Application to Tool Tracking System - Theory
The experiment used the same sensors and trajectory that were used in Section 5.4, and the
position errors of the tool tip were compared when the position of the tool tip is known (e.g., when
the tool fastens a bolt). Although the IMU was not factory calibrated, it was calibrated before use.
The last experiment, Figure 6-15, mimics the encoder-based position sensor used in Section 5.4.
Based on this experiment, the variables for the fastened bolt tracking system experiment are chosen.
The filter with 20 particles is used for this application because Figure 6-15 shows no significant
difference in the orientation error between the filter with 20 particles and the filter with 80 particles.
In order to apply the PF-KF tracking system to the fastening tool tracking application, the system
should be able to identify the fastened bolt. First, the fastening action was identified by using Rule 5104
4 to Rule 5-7 in Table 5-2. When the fastening action was detected, the tool tracking system should
identify the fastened bolt. To narrow down the possible fastened bolt, the system identifies which
bolts are within the position sensor error range ( PSEn ) by using (5-17). If only one bolt satisfies (517), the system concludes that the tool fastens that bolt. When more than one bolt satisfies (5-17), the
orientation and position of each particle and the possible bolt positions are used to identify the
fastened bolt. Let the calculated tool tip position components of particle i at time tk be Txki , Tyki ,
and Tz ki . Then, the position error of particle i with respect to Bolt n at time tk is
PEPni = ((Txki − n x ) 2 + (Ty ki − n y ) 2 + (Tz ki − n z ) 2 ) 0.5 .
(6-18)
The bolt with the minimum PEPni value is chosen as the fastened bolt for particle i. When all the
particles indicate that Bolt n is being fastened, the system concludes that the tool is fastening Bolt n.
Then, each particle is corrected by using the orientation correction method in subsection 5.3.4, which
uses the position measurements from the position sensor and the position and orientation of the
fastened bolt. When the orientation is corrected, all particles have the same orientation because the
position measurements and the position and orientation of the fastened bolt are the same for all
particles. In order to keep the variety of orientation particles, the particles are resampled immediately
after the orientation correction with 1/10th of the original process noise value.
The advantage of the PF is that the initial states are not required to be known. When the initial
orientation is unknown, the possible initial orientations can be calculated using the method in 6.3.2,
which utilizes accelerometer measurements to estimate the tilt angles. In order to find the correct
position of the tool tip, the tool tracking system waits until the first bolt is fastened if the initial
orientation is unknown. When the first fastening action is detected and only one bolt satisfies (5-17),
the system concludes that the tool fastens that bolt and all the particles converge to one orientation.
However, when there are multiple possibilities for the fastened bolt, each possibility has the same
number of particles and each particle tracks the position of the tool tip. Each possibility of the
fastened bolt sequence resamples within its own sampling pool to avoid a possibility of the fastened
bolt sequence disappear due to low weight. In order to evaluate the fastened bolt sequence, total
accumulated position error ( TAPEki ) is used, which is defined as
TAPE ki +1 = TAPE ki + {( Ppxi ,k − Pkxi ,k ) 2 + ( Ppyi ,k − Pkyi ,k ) 2 + ( Ppzi ,k − Pkzi ,k ) 2 }0.5 .
105
(6-19)
TAPEki is zeroed when there are multiple possible fastened bolts for the first time or when there is
one possibility of previous fastened bolt and multiple possible fastened bolt exist currently. When the
next fastening action is detected, each particle identifies the fastened bolt by identifying the bolt with
the minimum PEPni . Since PEPni is calculated with respect to the almost absolutely correct value
(bolt position), it was weighted twenty times more than the accumulated position error. When the
fastening action is detected, TAPEki is updated as follows:
TAPEki +1 = TAPEki + 20 × PEPni .
(6-20)
When all the particles indicate that they are fastening the same bolt, the system chooses the particle
with the minimum TAPEki value as the most probable possibility. Then, the system outputs the bolt
fastened sequence of the chosen particle, and all TAPEki values are set to zero. However, if there are
still multiple possibilities, TAPEki keeps accumulating until there is only one possible fastened bolt.
When the calculated position error of a particle is higher than the position error limitation of the
PF-KF method, the particle should be removed because this particle has an incorrect path. Therefore,
the maximum position error of the tool tip should be identified. From Figure 6-15, the maximum
error of the PK-PF system with 20 particles is 2°. However, since this 2° error is calculated from the
particle with the highest weight, an extra 2° of orientation error margin is added to ensure that all the
possible particles that fasten the bolt are included. Thus, 4° is chosen as the maximum orientation
error of the PK-PF system with 20 particles. Since the maximum position measurement error is
0.007m, the maximum position error of the tool tip position is
MPE = 165mm × sin(2 + 2°) + 7 mm = 18.5mm = 0.0185m .
(6-21)
From (6-21), it is concluded that all the bolts should be at least 0.037 (0.0185 × 2) m apart for the
system with 20 particles to identify the fastened bolt correctly. In case there are four possible
fastened bolt positions, each possibility has five particles. In this case, the maximum orientation error
is 4° as shown in Figure 6-15. Then, the MPE with a 2° error margin can be written as
MPE = 165mm × sin(4 + 2°) + 7 mm = 24.2mm = 0.0242m .
(6-22)
Therefore, when MPE is greater than 0.025 m, the ith particle is most likely not fastening Bolt n.
However, to keep more variety, 0.035 m is used as the position error limitation. Therefore, if the
106
distance between Bolt n and the particle i is greater than 0.035 m, the system removes this particle
and replaces it with a particle whose PEPni value is less than 0.035 m. To reduce this position error
limitation, more particles can be used. For example, if 80 particles are used, MPE becomes 0.0185 m
instead of 0.0242 m when there are four possibilities; thus, the position error limitation is lowered and
better accuracy can be achieved. Therefore, it is beneficial to use more particles when the bolts are
closely placed.
6.5.2 Experiment Results
Four different scenarios with two different tool trajectories are examined for the tool tracking
experiments. In the first scenario, the results using the KF-based system described in Section 5.4 and
the results using the PF-KF-based system with 20 particles are compared. For the second scenario,
the tool was moved beyond the angular velocity measurement limitation and the experimental results
of the two methods were compared. The third experiment shows how the PF-KF-based system finds
the correct orientation when the initial orientation is completely unknown. The last experiment shows
the scenario when the initial orientation is unknown and there are multiple possible fastened bolts.
For the first scenario, the PF-KF-based system with 20 particles was used in the tool tracking
application, and the results are compared with those using the KF-based system in Chapter 5. Table
6-3 summaries the position error using the KF-based system and using the PF-KF-based system with
20 particles. The results show that the PF-KF-based system has generally a higher accuracy than the
KF-based intelligent system.
In order to see how accurately the PF-KF-based system can estimate the orientation when the
angular velocity is slightly unreliable, the tool was rotated beyond the angular velocity measurement
range of the gyros. Figure 6-16 shows the angular velocity of each axis for the second experiment.
Figure 6-16 (a) depicts the entire time range of the experiment and Figure 6-16 (b) displays the
duration that the angular velocity of the tool is beyond the linear range of the gyros. The dashed lines
in the figures represent the measurement limit of gyros, which is ±75º/sec. When the angular velocity
is beyond this rate, the nonlinearity can be higher than the specification. The figures indicate that the
angular velocities in the x-axis and the y-axis are far beyond the linear range. For the y-axis, the
angular velocity measurements between 67.7 second to 69 second are cut off because the angular
velocity output is beyond the measurement limit. The comparison results of the second test are
summarized in Table 6-4.
107
Table 6-3: Position error comparison between the KF-based system and the PF-KF-based
system.
Tool
Time
position
(s)
Position error with the KF-based Position error with the PF-KF-based
system (mm)
system with 20 particles (mm)
X-axis
Y-axis
Z-axis
Total
X-axis
Y-axis
Z-axis
Total
Bolt 1
19
-2
0
6
6
-3
6
-2
7
Bolt 4
43
-12
-6
-6
15
-6
4
-2
7
Bolt 5
85
-2
-8
2
8
-5
5
4
8
Bolt 6
89
-4
-2
2
5
-4
2
5
7
Bolt 7
184
3
-6
7
10
1
1
6
6
Finish
204
-1
-6
0
6
-1
-2
-5
5
In Table 6-4, both systems correctly identified the fastened bolt. Most of the bolts exhibit a low
position error, but the error of Bolt 5 is much higher than that of the others due to the angular velocity
measurement error between 65 second and 69 second. This error resulted in 34 mm when the KFbased system is used. However, when the PF-KF-based system was used, the error is 25 mm. This is
due to the fact that the KF is suitable for zero-mean Gaussian noise while PF can adapt to any noise
distribution.
The orientation error due to angular velocity measurement error can be considered as an
uncertainty. Thus, the error can be reduced when a higher process noise is chosen and more particles
are placed to compensate for the high uncertainty. Table 6-5 shows the position error of the PF-KFbased system with different numbers of particles and covariance. In general, there is no significant
difference among the nine scenarios that are presented in Table 6-5 except for Bolt 5. When 20
particles are used, or the covariance is 0.006, there is no significant difference. When the number of
particle and covariance are increased, the position of Bolt 5 is estimated more accurately. However,
when the number of particles is changed from 40 to 60, there is no significant difference in the
position accuracy. This experiment shows that when the uncertainty increases, both the measurement
covariance and the number of particles should be increased to achieve a better result.
108
x-axis
225
225
150
150
75
75
0
0
-75
-75
y-axis
-150
0
20
40
60
80
100
120
-150
65
225
225
150
150
75
75
0
0
-75
-75
-150
0
20
40
60
80
100
120
-150
65
66
67
68
69
70
66
67
68
69
70
66
67
68
69
70
z-axis
225
150
150
75
75
0
0
-75
-75
-150
0
20
40
60
80
100
120
-150
65
Time (sec)
Time (sec)
Figure 6-16: The angular velocity (°/sec) of each axis: (a) the entire time span and (b) between
65 second and 70 second.
Table 6-4: Position error comparison between the KF-based system and the PF-KF-based
system when the angular velocity components have high errors.
Tool
Time
position
(sec)
Bolt 1
Position error with the KF-based Position error with the PF-KF-based
system (mm)
system with 20 particles (mm)
X-axis
Y-axis
Z-axis
Total
X-axis
Y-axis
Z-axis
Total
11
-6
2
4
7
-6
2
-3
7
Bolt 2
21
-5
0
-2
5
-4
0
0
4
Bolt 3
27
-2
6
0
6
0
1
3
3
Bolt 4
34
-5
3
5
8
-5
3
2
6
Bolt 5
73
-7
-10
-32
34
-6
-6
-23
25
Bolt 6
79
-5
3
-8
10
-5
3
-2
6
Bolt 7
89
-1
0
10
10
-1
0
-1
1
Bolt 8
102
0
12
1
12
-3
4
4
6
Finish
135
-2
4
-2
5
2
1
4
4
109
Table 6-5: Total position error comparison among the PF-KF-based system with different
numbers of particles and different covariance when the angular velocity components have high
errors.
20 particles
40 particles
60 particles
Covariance
0.006
0.009
0.012
0.006
0.009
0.012
0.006
0.009
0.012
Bolt 1
7
7
7
7
7
7
7
7
7
Bolt 2
4
5
6
4
4
5
4
4
4
Bolt 3
3
1
3
1
5
2
1
1
0
Bolt 4
6
6
6
6
6
7
6
6
6
Bolt 5
25
29
22
24
19
8
23
19
9
Bolt 6
6
6
8
8
7
6
6
6
6
Bolt 7
1
1
1
1
2
1
0
2
1
Bolt 8
6
6
7
5
8
8
5
5
5
Finish
4
10
5
5
6
7
5
8
2
The third experiment shows the case where the trajectory is the same as the first experiment but
the initial orientation is unknown. The experimental results are shown in Figure 6-17. The two
rectangles symbolize the tool (clearly shown at 19 second) and the small black dot signifies the
position sensor measurement. The “x” marks represent the calculated positions of the tool tip. When
the tool identifies the fastened bolt, two hexagrams are marked on the bolt position. First, the
orientation of the tool was randomly selected. At 1 second, the orientation of the tool was estimated
using (6-19), utilizing the average accelerometer measurement of each axis. Since the yaw angle is
unknown, the particles were equally spread. Even if the tilt angle is not corrected this way, the full
orientation can be identified when the first fastened bolt is identified. At 18.8 second, there are still
20 different possibilities of orientations, but when the system identified the fastening bolt at 19
second, the orientation converges to one solution because only one bolt satisfy (5-17). The remaining
figures in Figure 6-17 indicates that the particles spread over time, but the particles are very closely
placed immediately after the fastened bolt is identified.
110
Time: 0 sec
Time: 1 sec
0.2
0.2
0
0
-0.2
-0.2
-0.4
-0.4
-0.6
-0.6
-0.4
-0.2
0
0.2
-0.6
Tool
Tip
-0.6
Time: 18.8 sec
-0.4
0.2
0
0
Position
sensor
-0.2
-0.2
-0.4
-0.4
-0.6
-0.4
-0.2
0
0.2
-0.6
Time: 42.6 sec
0.2
0
0
-0.2
-0.2
-0.4
-0.4
-0.4
-0.2
0.2
0
0.2
0
0.2
Tool
-0.4
-0.2
Time: 42.8 sec
0.2
-0.6
0
Time: 19 sec
0.2
-0.6
-0.2
0
0.2
111
-0.6
-0.4
-0.2
Time: 84.8 sec
Time: 85 sec
0.2
0.2
0
0
-0.2
-0.2
-0.4
-0.4
-0.6
-0.4
-0.2
0
0.2
-0.6
0.2
0
0
-0.2
-0.2
-0.4
-0.4
-0.4
-0.2
0
0.2
-0.6
Time: 183.6 sec
0.2
0
0
-0.2
-0.2
-0.4
-0.4
-0.4
-0.2
0
0.2
-0.4
-0.2
0
0.2
0
0.2
Time: 204 sec
0.2
-0.6
-0.2
Time: 88.8 sec
Time: 88.6 sec
0.2
-0.6
-0.4
0
0.2
-0.6
-0.4
-0.2
Figure 6-17: Tracking the fastened bolts when the initial orientation is unknown.
112
For the fourth experiment, four additional bolt positions (Bolt 9 to Bolt 12) are added so that
multiple possibilities for the fastened bolt can be presented. All bolt positions are follows.
Table 6-6: Bolt locations.
Bolt number
Bolt position
Bolt number
Bolt position
Bolt 1
(-0.4. -0.34, 0)
Bolt 7
(0.05, -0.24, 0)
Bolt 2
(-0.4. -0.44, 0)
Bolt 8
(0.05, -0.34, 0)
Bolt 3
(-0.3. -0.34, 0)
Bolt 9 (added)
(-0.46. -0.24, 0)
Bolt 4
(-0.3. -0.44, 0)
Bolt 10 (added)
(-0.41. -0.3, 0)
Bolt 5
(-0.05, -0.24, 0)
Bolt 11 (added)
(-0.4. -0.39, 0)
Bolt 6
(-0.05, -0.34, 0)
Bolt 12 (added)
(-0.3 -0.4, 0)
Three bolt positions are added so that four possible fastened bolt positions exist when the first
fastening action is detected, and one bolt position is added so that two possible fastened bolt positions
exist when the second fastening action is detected. Figure 6-18 shows the experimental results. At 0
second, the orientation is unknown, so randomly chosen orientations were selected as the initial
orientation. At 19 second, the first fastening action was detected, and there are four possible fastened
bolt positions (Bolt 1, Bolt 9, Bolt 10, and Bolt 11) that satisfy (5-17). For each possible fastened bolt
position, a square mark is placed. Since there are four possible fastened bolts, five particles are
allocated to each possibility.
At 42.6 second, there are four sets of possible positions of the tool tip, and it appears three
possible fastened bolts exist (Bolt 3, Bolt 4, and Bolt 12, whose previous fastened bolts are Bolt 9,
Bolt 1, and Bolt 10 respectively). However, only two bolts (Bolt 4 and Bolt 12) satisfy (5-17). The
particles whose previous fastened bolts are Bolt 10 and Bolt 1 are fastening Bolt 12 and Bolt 4
respectively (the particles that fastened Bolt 10 and then fastened Bolt 12 in sequence will be denoted
as Bolt 10-12 hereafter). Since Bolt 3 did not satisfy (5-17), the particles that previously fastened
Bolt 9 were either resampled or assumed fastening Bolt 12 based on the distance between the particle
and the position of Bolt 12. If the particle is placed more than 0.035 m from Bolt 12, this particle has
likely chosen incorrect previous fastened bolt. Therefore, the particle is removed and replaced with a
random particle that has less than a 0.035 m error from the possible fastened bolt. When the particle
is less than 0.035 m from Bolt 12, it was assumed that the particle is fastening Bolt 12 (Bolt 9-12).
113
The particles previously fastened Bolt 11 were also either resampled or assumed fastening Bolt 4
(Bolt 11-4).
Time: 0 sec
Time: 19 sec
Time: 42.6 sec
0.2
0.2
0.2
0
0
0
-0.2
-0.2
-0.4
-0.4
-0.2
-0.4
-0.6
9
1
11
2
10
-0.4
3
12
4
-0.2
5
7
6
8
0
0.2
-0.6
Time: 42.8 sec
-0.4
-0.2
0
0.2
-0.6
Time: 84.8 sec
0.2
0.2
0
0
0
-0.2
-0.2
-0.2
-0.4
-0.4
-0.4
-0.4
-0.2
0
0.2
-0.6
Time: 88.8 sec
-0.4
-0.2
0
0.2
-0.6
Time: 183.6 sec
0.2
0.2
0
0
0
-0.2
-0.2
-0.2
-0.4
-0.4
-0.4
-0.4
-0.2
0
0.2
-0.6
-0.4
-0.2
0
0
0.2
-0.4
-0.2
0
0.2
Time: 204 sec
0.2
-0.6
-0.2
Time: 85 sec
0.2
-0.6
-0.4
0.2
-0.6
-0.4
-0.2
0
0.2
Figure 6-18: Fastened bolt detecting sequence. Square blocks indicate possible fastened bolts
and two hexagrams on the bolt signify the fastened bolts.
114
At 84.8 second, there is only one possible fastened bolt, and the two possibilities are very close to
Bolt 5. When the fastening action was detected at 85 second, the total accumulated position errors
( TAPEki ) are compared to identify the fastened bolt sequence. At this point, the possible fastened bolt
sequences could be a) Bolt 9-12-5, b) Bolt 10-12-5, c) Bolt 1-4-5, d) Bolt 11-4-5, e) Bolt 1-12-5, f)
Bolt 10-4-5. The system concludes that the particle with the Bolt 1-4-5 sequence is the correct
fastened bolt sequence because this particle has the minimum total accumulated position error. Thus,
Bolt 1, Bolt 4, and Bolt 5 are marked with two hexagrams on the bolt position at 85 second, and the
other possibilities (square marks) are removed. This is the correct sequence as shown in Table 6-3.
After identifying the true fastened bolt sequence, the system can identify the rest of the fastened bolts
without any multiple possibilities. This experiment was repeated twenty times, and the system
identified the correct fastened bolt sequence in each trial.
The previous experiment was repeated with the trajectory illustrated in Table 6-5. At 10.8 second,
four possible fastened bolt positions exist (Bolt 1, Bolt 9, Bolt 10, and Bolt 11). When the next
fastening action was detected at 20.8 second, five bolts satisfied (5-17) (Bolt 1, Bolt 2, Bolt 9, Bolt 10,
and Bolt 11), but the possible fastened bolts are reduced to three positions based on the particle
positions (Bolt 1, Bolt 11, and Bolt 2). Then, the possible bolt sequence was reduced to three (Bolt 91, Bolt 10-11, and Bolt 1-2) because the sequence possibility of Bolt 11-2 is eliminated due to a high
position error (more than 0.035 m). When the next fastening action was detected, the system
concludes that the tool is fastening Bolt 3, and the system identifies the previous fastened bolts
according to the total accumulated position error. Since the sequence with Bolt 1-2-3 has the
minimum total accumulated position error, the system concludes that the fastening sequence is Bolt 12-3. This is the correct fastened sequence as shown in Table 6-5. After the correct path is found, the
system also estimate the rest of the fastened bolts in the correct sequence.
115
Time: 10.8 sec
Time: 0 sec
Time: 20.6 sec
0.2
0.2
0.2
0
0
0
-0.2
-0.2
-0.4
-0.4
-0.2
-0.4
-0.6
9
1
11
2
10
-0.4
3
12
4
-0.2
5
7
6
8
0
0.2
-0.6
-0.4
-0.2
0
0.2
-0.6
Time: 26.4 sec
Time: 20.8 sec
0.2
0.2
0
0
0
-0.2
-0.2
-0.2
-0.4
-0.4
-0.4
-0.4
-0.2
0
0.2
-0.6
-0.4
-0.2
0
-0.2
0
0.2
Time: 134.8 sec
0.2
-0.6
-0.4
0.2
-0.6
-0.4
-0.2
0
0.2
Figure 6-19: Fastened bolt detecting sequence. Square blocks indicate possible fastened bolts
and two hexagrams on the bolt signify the fastened bolts.
6.6 Conclusion
This chapter presented a position/orientation estimation method using a KF-PF combination and
how this method was applied to the fastening tool tracking system. The proposed KF-PF combination
method uses the PF to estimate the object orientation while the KF is used to estimate the position. In
addition, an expert system was developed to correct the angular velocity bias as well as to identify the
stationary state of the object.
The experimental results indicate that the orientation errors using the proposed method are
significantly reduced compared to the errors using the EKF estimation method. Two factors that
affect the orientation error were studied: (i) the number of particles and (ii) position sensor noise. As
the number of particles increases, the orientation error by using the proposed method tends to
decrease.
However, increasing the number of particles requires higher computational costs.
116
Therefore, the number of particles should be chosen to achieve a tradeoff between accuracy and
computational efficiency.
The experimental results demonstrate that the proposed method can
estimate orientation even in the presence of Gaussian or random position sensor noise. However, the
experimental results also reveal that the position noise decreases the orientation estimation accuracy.
The experiments demonstrated that the proposed filter can find the correct orientation of the object
even when the initial orientation is completely unknown.
The KF-PF-based system yields a higher accuracy, and the orientation does not increase over time.
The test results of KF-PF-based system indicate that the proposed system can estimate the fastened
bolt even when the bolts are closely placed (0.04 m apart). When the bolts are more than 0.04 m
apart and the initial orientation is known, the fastened bolt can be correctly identified.
For a
workpiece that requires a specific fastening order such as an engine mount, the KF-PF-based system
can be used as a fastening tool control system. When the operator tries to fasten a bolt out of
sequence, the tool control system can be programmed so that the power of the tool is off. In addition,
the tool control system can have multiple torque settings based on the bolt position. For example,
when two bolts have the same bolt size but each requires different torque, one tool can be used to
fasten both bolts by using the control system that controls torque based on the tool tip location.
The KF-PF-based system can yield accurate orientation estimation even when the gyros have a
small error and when the initial orientation is unknown. When high computational power is available,
the KF-PF combination system is the better choice to estimate the position and orientation of the tool
than the KF system described in Chapter 5.
117
Chapter 7
Thesis Contributions and Future Work
7.1 Thesis Contributions
In this thesis, four contributions related to fastening tool tracking systems are proposed. These
objectives are summarized as follows.
•
An accurate triaxial calibration method that has a simple procedure.
•
A tool tracking system that does not require a line of sight by using an IMU and a triaxial
magnetometer.
•
A tool tracking system by using KFs and a fuzzy expert system with an IMU and an
encoder position sensor.
•
A tool tracking system by using the KF-PF combination with an IMU and an encoder
position sensor.
An accurate calibration method for an IMU is important especially when the system does not have
a position sensor to correct the position estimation. The newly developed triaxial calibration method
only requires placing the sensor in six different tilt angles for calibration and offers a high accuracy.
Three different fastening tool tracking systems using an IMU and one additional sensor are
developed. The first system relies on an IMU and a triaxial magnetometer. In order to reduce the
position error, an expert system is used to correct the velocity, position, and orientation error. The
advantage of this low-cost system is that it does not require lines of sight and has a low computational
cost. However, even with the position error correction algorithm, the position error grows rapidly
over time. Therefore, this tracking system can only be used for a workpiece that has different bolt
orientations or the distances between the bolts are far from each other.
To overcome the disadvantages of such a system, an encoder-based position sensor is employed
instead of a triaxial magnetometer. The KF is developed to hybridize the IMU and the position
sensor. A fuzzy expert system is utilized to identify the fastening action and correct the position and
orientation. Although this system is computationally inexpensive, the orientation error grows over
time, which affects the position estimation of the tool tip. This system is useful for applications
where the bolts are not too closely apart or the fastening sequence is not important.
118
The third system uses the KF-PF combination to hybridize a position sensor and an IMU. The
experimental results demonstrate that the maximum orientation error of the presented system with 20
particles is about 2º, and the orientation error of the system does not grow over time. Therefore, the
KF-PF combination system can be applied to build a tool control system so that the tool is powered
on only when the tool is in designated positions. Then, an operator cannot fasten bolts out of
fastening sequence. The KF-PF-based system can find the fastened bolt accurately even when the
initial orientation is unknown. In addition, the proposed method can accurately estimate orientation
by utilizing enough number of particles and high process noise even when the gyros have a small
error. The advantages and disadvantages of each system are summarized in Table 7-1.
Table 7-1: Advantages and disadvantages of the system presented in this thesis.
System
Advantages
Disadvantages
Option 1:
1. No line of sight requirement.
1. Low accuracy.
IMU and magnetometer 2. Low computational cost.
using an expert system
3. Low cost.
2. The bolts must be a long
distance
orientations
apart
or
the
of
the
bolts
should be different.
Option 2:
IMU
1. Computationally inexpensive.
and
1. A line of sight is required.
encoder 2. The fastened bolt can be accurately 2. The position and orientation
position sensor using
found when the bolts are some
errors of the tool tip slowly
KF and fuzzy expert
distance apart.
grow over time.
system
Option 3:
IMU
1. The tool tip position can be 1. A line of sight is required.
and
encoder
accurately estimated.
2. High computational cost.
position sensor using 2. The tool tip position can be
KF-PF
and
expert
system
estimated even when the initial
orientation is unknown.
3. A fastening tool control system can
be built.
7.2 Future Work
This thesis describes the developments of tool tracking systems that utilize one IMU and one
additional sensor. The most attractive option for a fastening tool tracking system is to use a high
accuracy position sensor without a line of sight requirement. Since an IMU must be attached to a
119
fastening tool, the sensor has to be small and lightweight. This limitation led to the use of a MEMS
IMU whose accuracy is limited. However, since the MEMS IMU technology is advancing rapidly,
more accurate MEM IMUs can be used to measure the acceleration and angular velocity. This, in
turn, will lead to a more accurate position and orientation estimations especially when Option 1 in
Table 7-1 is selected.
In this thesis, an encoder-based position sensor was chosen as the hybridization for Option 2 and
Option 3. In the future, other position sensor options can be studied. Although it is challenging to
use a magnetic position sensor in the automotive industry, this sensor is a good option. This is due to
the fact that a magnetic position sensor has a high accuracy and does not require a line of sight. Also,
an UWB position sensor can be a good option when lower position accuracy is required. When an
UWB position sensor is hybridized with an IMU, the position accuracy will be greatly improved.
Option 3 in Table 7-1 offers a possible tool torque control system that can be used to control the
power of the tool based on the position of the tool tip. Therefore, this work can be extended to the
development of a tool torque control system which outputs the torque based on the position and
orientation information obtained from the system presented in Option 3.
In this thesis, fastening tool tracking systems are investigated as an application of a remote
sensing.
However, the application of remote sensing is not limited to tool tracking systems.
Therefore, a general hybrid method of the KF that combines an IMU and a position sensor is
investigated in Chapter 5, and a general hybrid method of a KF-PF combination was developed to
estimate the position and orientation in Chapter 6.
In the future, the remote sensing system using the KF-PF combination can be applied to estimating
the position and orientation of a rehabilitation patient’s body part by using an ultrasonic position
sensor, and electromagnets, and an IMU. In such an application, a MEMS IMU is attached to the
position of interest and an ultrasonic emitter is attached to the IMU. When the lines of sight are
available, the position and orientation can be estimated with the position sensor and the IMU using
the KF-PF combination method. Meanwhile, a neural network system is used to create a magnetic
field map related to the position. Since the magnetic field can be changed when an electrical device is
switched on or off, the map might be updated if necessary. When occlusion occurs, the position
information can be retrieved from the magnetic field map, and the KF-PF combination system can be
used to estimate the position and orientation of the body part. By using the position and orientation
estimations, the rehabilitation system will direct the movement of the patient on a monitor.
120
Appendix A: Quaternions
The quaternion was developed by William Rowan Hamilton to extend 3D vector algebra. He
introduced a concept of a hyper-complex number of rank 4: one real number and three imaginary
numbers (i, j, and k). A quaternion q can be written as:
q = q0 + q1i + q2 j + q3 k ,
(A-1)
q = q0 + qv ,
(A-2)
or it can also be written as:
where q0 is the scalar part of the quaternion, and qv is the vector part (imaginary part) of the
quaternion. When q0 is zero, the quaternion is entirely imaginary and is called a pure quaternion.
The imaginary numbers follow the right hand rule such that
ij = k , ji = −k , jk = i , kj = −i , ki = j , ik = − j .
Also, they follow imaginary number rules such that
i 2 = −1 , j 2 = −1 , k 2 = −1 .
In addition, quaternions follow the basic rules of vector addition and multiplication. Let two
quaternions p and q be
p = p0 + p1i + p2 j + p3 k ,
(A-3)
q = q0 + q1i + q2 j + q3 k .
(A-4)
Then, the sum of the two quaternions becomes
p + q = ( p0 + q0 ) + ( p1 + q1 )i + ( p2 + q2 ) j + ( p3 + q3 )k ,
(A-5)
and the multiplication becomes
pq = ( p0 + p1i + p2 j + p3 k )(q0 + q1i + q2 j + q3 k )
= ( p0 q0 + p1q0 i + p2 q0 j + p3 q0 k ) + ( p0 q1i + p1q1i 2 + p2 q1ij + p3 q1ik )
+ ( p0 q2 j + p1q2 ji + p2 q2 j 2 + p3 q2 jk ) + ( p0 q3 k + p1q3 ki + p2 q3 kj + p3 q3 k 2 ) .
(A-6)
= ( p0 q0 − p1q1 − p2 q2 − p3 q3 ) + ( p0 q1 + p1q0 + p2 q3 − p3 q2 )i
+ ( p0 q2 + p2 q0 + p3 q1 − p1q3 ) j + ( p0 q3 + p3 q0 + p3 q3 − p3 q3 )k
As it can be seen by inspection, commutivity is preserved for addition but is not preserved for
multiplication. The conjugate of quaternions q, denoted as q*, is given by
121
q* = q0 − qv = q0 − q1i − q2 j − q3 k .
(A-7)
In order to use quaternions to determine the orientation in 3D vector space, the real part of the
quaternion must be zero (i.e., v = 0 + v1i + v2 j + v3 k ). This also means that the 3D vector v is a pure
quaternion once rotated by quaternion q. Let a quaternion q represent a four dimensional rotation
vector, and a vector v represent a vector in a three dimensional space. Then, the quaternion after
rotation becomes
q ⋅ v = (q0 + qv )(0 + vv )
= q v ⋅ v v + q0 ⋅ v v + q v × v v .
(A-8)
Eq. (A-8) cannot be a pure quaternion unless qv ⋅ vv = 0 . Hence a single multiplication of a
quaternion does not yield a three dimensional vector.
If a 3D vector v is multiplied by two
quaternions, p and q, the vector v after multiplication is written as
q ⋅ v ⋅ p = (q0 + qv )(0 + vv )( p0 + pv ) .
(A-9)
The real part of (A-9) has to be zero in order to make the multiplication a pure quaternion. The real
part calculation is expressed as
− p0 ( qv ⋅ v v ) − q 0 ( v v ⋅ p v ) + ( q v × p v ) ⋅ v v = 0 .
(A-10)
If p 0 = q 0 , (A-10) can be simplified as
− p0 (qv + pv ) ⋅ vv + (qv × pv ) ⋅ vv = 0 .
(A-11)
Eq. (A-11) is always true if pv = −qv . Therefore, the quaternion p becomes
p = p0 + pv = q0 − qv = q * .
(A-12)
Then, the vector v after rotation becomes
(q0 + q1i + q2 j + q3 k )(0 + xi + yj + zk )(q0 − q1i − q2 j − q3 k )
=0
2
2
2
2
+ {(q0 + q1 + q2 + q3 ) x + 2(q1q 2 − q0 q3 ) y + 2(q1q3 + q0 q 2 ) z}i
2
2
2
2
+ {2(q1q2 + q0 q3 ) x + (q0 − q1 + q2 − q3 ) y + 2(q2 q3 − q0 q1 ) z} j
2
2
2
2
+ {2(q1q3 − q0 q2 ) x + 2(q 2 q3 + q0 q1 ) y + (q0 − q1 − q 2 + q3 ) z}k
This can be written in the matrix form as follows:
122
.
(A-13)
qvq* = Qv ,
0
0
0 q 2 + q 2 − q 2 − q 2
0
1
2
3
where Q = 
0
2( q1q2 + q0 q3 )

2(q1q3 − q0 q2 )
0
(A-14)

0

 x
2(q1q3 + q0 q2 ) 
and v =   .
 y
2(q2 q3 − q0 q1 ) 
 
2
2
2
2
q0 − q1 − q2 + q3 
z
0
0
2(q1q2 − q0 q3 )
2
2
2
q0 − q1 + q2 − q3
2(q2 q3 + q0 q1 )
2
When a quaternion q is a unity vector, it satisfies
2
2
q0 + qv = 1 .
(A-15)
cos 2 θ + sin 2 θ = 1 ,
(A-16)
Then, (A-15) can be rewritten as
where cos 2 θ = q0 2 and sin 2 θ = qv
2
A vector u is defined as a unit vector parallel to the vector part of quaternion q as
u=
qv
q
= v .
qv sin θ
(A-17)
Then, the unit quaternion q is
q = q0 + qv = cos θ + u sin θ .
(A-18)
Then, qvq * can be written as
qvq* = (q0 + qv )(0 + vv )(q0 − qv )
2
2
= ( q0 − q v ) v v + 2( q v ⋅ v v ) qv + 2 q 0 ( q v × v v ) .
(A-19)
Expressing v in terms of vp (parallel to qv) and vn (normal to qv) yields the following:
2
2
2
2
2
2
q ⋅ v ⋅ q* = (q0 − qv )(v p + vn ) + 2(qv ⋅ (v p + vn ))qv + 2q0 (qv × (v p + vn ))
2
2
= ( q 0 − qv ) v p + 2( q v ⋅ v p ) qv + 2 q 0 ( q v × v p ) + ( q 0 − qv ) v n + 2( qv ⋅ v n ) q v + 2 q0 ( qv × v n )
2
2
2
= (q0 − qv + 2 qv )v p + (q0 − qv )vn + 2q0 qv (u × vn )
.
= v p + (sin 2 θ − cos 2 θ )vn + 2 cos θ sin θ (u × vn )
= v p + cos(2θ )vn + sin(2θ )(u × vn )
(A-20)
123
Therefore, vector v after rotation is calculated by
qvq* = v p + cos( 2θ )vn + sin( 2θ )(u × vn ) ,
(A-21)
where u is a unit vector of qv. Consequently, u × vn is perpendicular to both qv and vn.
The relationship between arbitrary vector v and qvq* is illustrated by the geometry in Figure A-1
and Figure A-2. Eq. (A-21) illustrates that vector v is rotated by angle 2θ because the 3D vector v is
rotated by θ once about q and once about q*. In order to rotate v by θ, the rotation of quaternion q
needs to be halved. In other words,
q = q0 + qv = cos
θ
2
+ u sin
θ
2
= cos
θ
2
+ (u xi + u y j + u z k ) sin
θ
2
(A-22)
Therefore, each component of the quaternion q should be written as follows:
q0 = cos
θ
2
ux
θ
q1 =
sin
u
2
q2 =
q3 =
uy
u
sin
θ
2
uz
θ
sin
u
2
q
vn
vv
v
2θ
qvq
Figure A-1: Rotation of vector v by the angle of 2θ
θ about vector q.
124
(A-23)
qvnq
u vn
u vn sin2θ
2θ
vn sin2θ
vn
Figure A-2: Rotated vector component vn before and after rotation.
125
Appendix B: Gyro Calibration
For gyro calibration, the method presented in [16] is adapted because this method does not require
any additional sensor and the procedure is simple enough to be used in automotive industries. In this
method, a gyro is modeled as follows:
ω _ measaxis = GGaxis ⋅ ωaxis + GBaxis + GAS axis ⋅ Aaxis ,
(B-1)
where ω _ measaxis is the gyro measurement of each axis, GGaxis is the gyro gain of each axis, ω axis
is the true angular velocity, and GBaxis is the gyro bias of each axis, and GAS axis is the linear
acceleration sensitivity of each axis.
The linear acceleration sensitivity can be modeled as linear. When the sensor is stationary, the
angular velocity is zero, and the gyro output is equal to the gyro biases plus acceleration sensitivity
multiplied by the acceleration. When the accelerometer is calibrated, the IMU is placed stationary in
six tilt angles for at least two seconds. After accelerometer calibration, the six tilt angles can be
calculated. Then, the average acceleration that the gyro axis is subjected to during the stationary state
can be calculated.
By using the two average accelerations from the six tilt angles, the linear
acceleration sensitivity is calculated as
GAS axis =
ω _ A1axis − ω _ A2 axis
Avg _ A1 − Avg _ A2
,
(B-2)
where Avg _ A1 is the first average acceleration, ω _ A1axis is the corresponding gyro output of the
first acceleration, and Avg _ A2 is the minimum calculated acceleration and ω _ A min axis is the
corresponding gyro output. When the acceleration sensitivity is calculated, the gyro biases can be
calculated by substituting the gyro acceleration sensitivity and the subjected acceleration in (3-19).
In order to calculate the gyro gain, the angular velocity should be measured. In this method,
instead of utilizing an additional sensor to obtain the angular velocity, the integration of the angular
velocity is used. By integrating both sides of (B-1),
∫ (ω _ meas
axis
∫
∫
− GBaxis − GAS axis ⋅ Aaxis ) = GGaxis ⋅ ωaxis = GGaxis ⋅ ω axis .
(B-3)
Since the gyro measurement, gyro bias, acceleration and gyro acceleration sensitivity are known,
the left side of (B-3) is known. When the final orientation is Θ , (B-3) is written as
126
∫ (ω _ meas
axis
∫
− GBaxis − GAS axis ⋅ Aaxis ) = GGaxis ⋅ ω axis = GGaxis ⋅ Θ .
(B-4)
Then, the gain of the gyro can be calculated as
GGaxis =
∫ (ω _ meas
axis
− GBaxis − GAS axis ⋅ Aaxis )
Θ
.
(B-5)
Equation (3-4) is true only when the sensor is rotated in one axis. Therefore, when a gyro is
calibrated, the sensor should be placed on a smooth surface, and one side of the sensor should be
placed on a reference block as shown in Figure B-1. Then, the sensor is rotated 360º so that the same
sensor side faces the reference block after its rotation. Then, the final orientation ( Θ ) becomes 360º.
Detailed theory and test results of gyro calibration are reported in [16].
Rotation
axis
Reference
Block
IMU
Smooth
surface
Z
Y
X
Figure B-3: The calibration of z-axis gyro using a reference block.
127
Bibliography
[1]
X. Yun , E. R. Bachmann , R. B. McGhee , R. H. Whalen , R. L. Roberts , R. H. Knapp , A. J.
Healey, and M. J. Zyda “Testing and Evaluation of an Integrated GPS/INS System for Small
AUV Navigation,” IEEE Journal of Oceanic. Engineering, Vol. 24, Jul. 1999, pp. 396-404.
[2]
K. Kobayashi, K. C. Cheok, K. Watanabe, and F. Munekata, “Accurate Differential Global
Positioning System via Fuzzy Logic Kalman Filter Sensor Fusion Technique”, IEEE
Transactions on Industrial Electronics, Vol. 45 No. 3, June, 1998, pp.510-518.
[3]
G. Hyslop, D. Gerth and J. Kraemer, “GPS/INS Integration on the Standoff Land Attack
Missile (SLAM)”, IEEE AES Magazine, Jul. 1990, pp. 29–34.
[4]
N. Parnian and M. F. Golnaraghi, “Integration of Vision and Inertial Sensors for Industrial
Tools Tracking”, Sensor Review, Vol. 27, No. 2, 2007, pp.132-141.
[5]
R. Oktem, E. Aydin, and N. Cagiltay, “An Indoor Navigation Aid Designed for Visually
Impaired People”, 34th Annual Conference of the IEEE Industrial Electronics Society, IECON
2008, 10-13 November 2008, Orlando, Florida, USA. pp. 2982 – 2987.
[6]
Jing Yang, Eun-Seok Choi, Wook Chang, Won-Chul Bang, Sung-Jung Cho, Jong-Koo
Oh, Joon-Kee Cho, and Dong-Yoon Kim “A Novel Hand Gesture Input Device Based on
Inertial Sensing Technique”, IECON 2004, 2-6 November 2004, Busan, Korea. pp. 2786 –
2791.
[7]
Y. Tao, H. Hu, and H. Zhou, “Integration of Vision and Inertial Sensors for 3D ARM Motion
Tracking in Home-based Rehabilitation,” International Journal of Robotics Research, Vol. 26,
No. 6, 2007, pp. 607-624.
[8]
“Company website - PINpoint Information Systems”, [Online]. Available: http://www.
pinpointinfo.com/ [Accessed Feb. 17, 2010].
[9]
“Company website - peeperl+Fuchs”, [Online]. Available: www.am.pepperl-fuchs.com
/products/productfamily.jsp?productfamily_id=1730 [Accessed Feb. 17, 2010].
[10] Georg Ogris, Thomas Stiefmeier, Holger Junker, Paul Lukowicz, and Gerhard Tröster, "Using
Ultrasonic Hand Tracking to Augment Motion Analysis Based Recognition of Manipulative
Gestures", in Proceedings of the 9th IEEE International Symposium on Wearable Computers,
Osaka, Japan, October 18-21, 2005 .
[11] E. Foxlin, “Extended Draft Version of Chapter 8 in Handbook of Virtual Environment
Technology”, Kay Stanney, Editor, Lawrence Erlbaum Associates, 2002.
[12] Walid Abdel-Hamid, “Accuracy Enhancement of Integrated MEMS-IMU/GPS Systems for
Land Vehicular Navigation Applications”, PhD Thesis, University of Calgary, Calgary, AB,
Canada, January 2005.
[13] P. H. LaFond, “Modeling for Error Reduction Accelerometers”, in Proceedings of IEEE
PLANS 1992. Mar. 23-27, 1992, pp. 126-132.
128
[14] H. J. Luinge and P. H. Veltink, “Inclination Measurement of Human movement using a 3-D
Accelerometer with Autocalibration”, IEEE Transactions on Neural Systems and Rehabilitation
Engineering 12, 2004, pp. 112-121.
[15] “3DM-GX1 Sensor Performance Improvement with Temperature Compensation”, Microstrain,
[Online].
Available:
http://www.microstrain.com/pdf/Comparision%20With%20and%
20Without%20Temp%20Comp.pdf [Accessed Feb.17, 2010].
[16] F. Ferraris, U. Grimaldi, and M. Parvis, “Procedure for Effortless In-field Calibration of Threeaxis Rate Gyros and Accelerometers”, Sensors and Materials, 1995, Vol. 7, No. 5, pp. 311-330.
[17] D. Giansanti, G. Maccioni, V. Macellari, “Guidlines for Calibration and Drift Compensation of
a Wearable Device with Rate-Gyroscopes and Accelerometers”, in Proceedings of the 29th
Annual International Conference of the IEEE EMBS, Lyon, France, Aug. 22-26, 2007, pp.
2342-2345.
[18] P. Lang and A. Pinz, “Calibration of hybrid vision / inertial tracking systems”’ in Proceedings
of 2nd InerVis Workshop on Integration of Vision and inertial Sensors, Apr. 18, 2005.
[19] Akira Umeda, Mike Onoe, Kohji Sakata, Takehiro Fukushia, Kouichi Kanari, Hiroshi Iioka,
and Toshiyuki Kobayashi, “Calibration of Three-axis Accelerometers using a Threedimensional Vibration Generator and Three Laser Interferometers”, Sensors and Actuators A,
114, 2004, pp. 93–101.
[20] E. L. Renk, M. Rizzo, W. Collins, F. Lee, and D. S. Bernstein, “Calibrating a Triaxial
Accelerometer-magnetometer - Using Robotic Actuation for Sensor Reorientation During Data
Collection”, IEEE Control Systems Magazine, Vol. 25, No. 6, 2005, pp. 86–95.
[21] A. Kim and M. F. Golnaraghi, “Initial Calibration of an Inertial Measurement Unit Using an
Optical Position Tracking System”, Proceedings of the PLANS 2004, IEEE Position Location
and Navigation Symposium, April 26–29, 2004, pp. 96-101.
[22] Zhuxin Dong, Guanglie Zhang, Yilun Luo, Chi Chiu Tsang, Guangyi Shi, Sze Yin Kwok, Wen
J. Li, Philip H. W. Leong, and Ming Yiu Wong, “A Calibration Method for MEMS Inertial
Sensors Based on Optical Tracking”, in Proceedings of 2007 IEEE International Conference on
Nano/Micro Engineered and Molecular Systems (IEEE-NEMS2007), Bangkok, Thailand, Jan.
16-19, 2007, pp. 542-547.
[23] H. J. Luinge and P. H. Veltink, “Inclination Measurement of Human Movement Using a 3-D
Accelerometer with Autocalibration”, IEEE Transactions on Neural Systems and Rehabilitation
Engineering 12, 2004, pp. 112-121.
[24] E-H. Shin and N. El-Sheimy, “A New Calibration Method for Strapdown Inertial Navigation
Systems”, Zeitschrift fűr Vermessungswesen, Vol. 127, No. 1, Jan. 2002, pp. 41–50.
[25] Z. F. Syed, P. Aggarwal, C. Goodall, X. Niu, and N. El-Sheimy, “A New Multi-position
Calibration Method for MEMS Inertial Navigation Systems”, Meas. Sci. Technol. Vol. 18, No.
7, July 2007, pp. 1897-1907.
[26] A. Lai, D. A. James, P. Hayes, and E. C. Harvey, “Semi-automatic Calibration Technique using
Six Inertial Frames of Reference”, in Proceedings of SPIE 5274, Perth, WA, Australia, Dec.
10-12, 2003, pp. 531-542.
129
[27] J. C. Lötters, J. Schipper , P. H. Veltink , W. Olthius, and P. Bergveld, “Procedure for In-use
Calibration of Triaxial Accelerometers in Medical Applications”, Sensors Actuators A 68,
1998, pp. 221-228.
[28] Z. C. Wu, Z. F. Wang, and Y. Ge, “Gravity Based Online Calibration for Monolithic Triaxial
Accelerometers' Gain and Offset Drift”, Proceedings of the 4th World Congress on Intelligent
Control and Automation 3, Shanghai, China, Jun. 10-14, 2002, pp. 2171-2175.
[29] L. Ojeda, H. Chung, and J. Borenstein, “Precision-calibration of Fiber-optics Gyroscopes for
Mobile Robot Navigation,” in Proceedings of 2000 IEEE Int. Conf. Robotics and Automation
San Francisco, CA, Apr. 24–28, 2000, pp. 2064-2069.
[30] Kim, and M. F. Golnaraghi, “A Quaternion-Based Orientation Estimation Algorithm Using an
Inertial Measurement Unit”, IEEE Position Location and Navigation Symposium 2004,
Monterey, CA, April 26 - 29, 2004, pp. 268-272.
[31] H. Zhou, H. Hu, and Y. Tao, “Inertial Measurements of Upper Limb Motion,” Journal of
Medicine, Biological Engineering and Computing, 2006, 44, pp. 479-487.
[32] H. J. Luinge P. H. Veltink, “Measuring Orientation of Human Body Segments Using Miniature
Gyroscopes and Accelerometers”, Medical & Biological Engineering & Computing 2005, Vol.
43, pp. 273-282.
[33] M. Koifman and I. Y. Bar-Itzhack, “Inertial Navigation System Aided by Aircraft Dynamics”,
IEEE Transactions on Control Systems Technology, Vol. 7, No. 4, July 1999, pp. 487 – 493.
[34] W. Abdel-Hamid, A. Noureldin, and N. El-Sheimy, "Adaptive Fuzzy Prediction of Low-Cost
Inertial-Based Positioning Errors", IEEE Transactions on Fuzzy Systems, Vol. 15, No. 3, Jun.
2007, pp. 519-529.
[35] R. Rahbari, B. W. Leach, J. Dillon, and C. W. de Silva, “Expert System for an INS/DGPS
Integrated Navigator Installed in a Bell 206 Helicopter”, IEEE Aerospace Conference, Big Sky,
MT, March 6–13, 2004 pp. 1569-1579.
[36] Lovren, N.; Pieper, J.K. “Error Analysis of Direction Cosines and Quaternion Parameters
Techniques for Aircraft Attitude Determination”, IEEE Transaction on Aerospace And
Electronic Systems, Vol. 34, No. 3, July 1998, pp. 983 – 989.
[37] Lekskulchai Pongsak, Masafumi Okada, Tetsuya Sinohara, and Yoshihiko Nakamura, “Attitude
Estimation by Compensating Gravity Direction”, In Proceedings of the 21th Annual
Conference of the Robotics Society in Japan, Sep. 20-23, 2003, 2A23.
[38] H. J. Luinge and P. H. Veltink, “Measuring Orientation of Human Body Segments Using
Miniature Gyroscopes and Accelerometers”, Medical & Biological Engineering & Computing
2005, Vol. 43, pp. 273-282.
[39] H. J. Luinge, P.H. Veltink, C.T.M. Baten, “Ambulatory Measurement of Arm Orientation”
Journal of Biomechanics, Vol. 40, No. 1, 2007, pp. 78-85.
[40] H. Rehbinder and X. Hu, “Drift-free Attitude Estimation for Accelerated Rigid Bodies”,
Automatica 40, 2004, pp. 653-659.
130
[41] Ashutosh Saxena, Gaurav Gupta, Vadim Gerasimov, and Sébastien Ourselin, “In Use
Parameter Estimation of Inertial Sensors by Detecting Multilevel Quasi-static States”, In
KES2005, Melbourne, Australia, September 2005, pp. 595-601.
[42] J-H Wang, and Y. Gao, “Multi-sensor Data Fusion for Land Vehicle Attitude Estimation using
Fuzzy Expert System”, Data Science Journal. Vol. 4, 2005, pp.127-139.
[43] L. Ojeda and J. Borenstein, “FLEXnav: Fuzzy Logic Expert Rule-based Position Estimation for
Mobile Robots on Rugged Terrain.” In Proceedings of the 2002 IEEE Int. Conference on
Robotics and Automation, May 11-15, 2002, Washington DC, USA, pp. 317-322.
[44] D. Roetenberg, P. J. Slycke, and P. H. Veltink, “Ambulatory Position and Orientation Tracking
Fusing Magnetic and Inertial Sensing”, Biomedical engineering, Vol. 54 No. 5, 2007, pp. 883890.
[45] M. Sabatini, “Quaternion-based Extended KF for Determining Orientation by Inertial and
Magnetic Sensing,” Biomedical Engineering, Vol. 53, No. 7, 2006, pp. 1346–1356.
[46] J. Hummel, M. Figl, C. Kollmann, H. Bergmann, and W. Birkfellner “Evaluation of a
Miniature Electromagnetic, Position Tracker” Medical Physics Vol. 29, No. 10, 2002, pp.
2205–2212.
[47] E. R. Bachmann, X. Yun, C. W. Peterson, “An Investigation of the Effects of Magnetic
Variations on Inertial/Magnetic Orientation Sensors”, in Proceedings of ICRA 2004, Apr. 26May 1, 2004, New Orleans, LA, pp. 1115-1122.
[48] Daniel Roetenberg, Henk J. Luinge, Chris T. M. Baten, and Peter H. Veltink, “Compensation
of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment
Orientation”, IEEE Trans on Neural Systems and rehabilitation engineering, Vol. 13 No. 3,
2005, pp. 395- 405.
[49] Daniel Roetenberg, Henk Luinge, and Peter Veltink, “Inertial and Magnetic Sensing of Human
Movement Near Ferromagnetic Materials” in Proceedings of 2nd IEEE and ACM International
Symposium on Mixed and Augmented Reality, ISMAR ’03, Tokyo, Japan, Oct. 7-10, 2003, pp.
268-269.
[50] C. T. M. Baten, H. J. Luinge, and H. V. Moerkerk, “Estimating Body Segment Orientation
Applying Inertial Sensing”, Neural Systems and Rehabilitation Engineering, Vol. 15, No. 3, pp.
469-471.
[51] “Firefly Prototype Specifications”, Firefly Motion Capture System, [Online]. Available:
http://www.cybernet.com/interactive [Accessed: Feb. 17, 2010]
[52] R. A. States and E. Pappas, “Precision and Repeatability of the Optotrak 3020 Motion
Measurement System”, Journal of Medical Engineering & Technology, Vol. 30, No. 1, Jan.
2006, pp.11 – 16.
[53] Lorin P. Maletsky, Junyi Sun, and Nicholas A. Morton, “Accuracy of an Optical Active-marker
System to Track the Relative Motion of Rigid Bodies”, Journal of Biomechanics, Vol. 40, No.
3, 2007. pp. 682-685.
131
[54] A. D. Wiles, D.G. Thompson, and D.D Frantz, “Accuracy Assessment and Interpretation for
Optical Tracking Systems”, In Proceedings of SPIE 5367, Medical Imaging, San Diego, CA,
United States, 2004, pp. 421-432
[55] N. Parnian, “Integration of Local Positioning System & Strapdown Inertial Navigation System
for Hand-Held Tool Tracking”, Ph.D. Thesis, University of Waterloo, 2008
[56] M. Ribo, M. Brandner, and A. Pinz. “A Flexible Software Architecture for Hybrid Tracking”,
Journal of Robotics Systems”, Vol. 21, No. 2, Feb. 2004, pp.53-62.
[57] Eric Foxlin and Leonid Naimark, “Miniaturization, Calibration & Accuracy Evaluation of a
Hybrid Self-Tracker”, Proc. of 2nd IEEE and ACM International Symposium on Mixed and
Augmented Reality, ISMAR ’03, Tokyo, Japan, Oct. 7-10, 2003, pp. 151-160.
[58] D. Roetenberg, P. Slycke, A. Ventevogel, and P.H. Veltink, “A Portable Magnetic Position and
Orientation tracker”, Sensors and Actuators A, Vol. 135, No. 2, Apr. 2007, pp. 426–432.
[59] Mason, A. Shaw, A. I. Al-Shamma'a, “Intelligent Radio Frequency Identification Positioning
using Wireless Sensor Networks”, Antennas and Propagation Conference, April 2-3, 2007,
Loughborough, UK. pp. 145-148.
[60] Andreas Stelzer, Alexander Fischer, Franz Weinberger, and Martin Vossiek, “RF-Sensor for a
Local Position Measurement System”, in Proceedings of SPIE, Vol. 5048, 2003, pp. 136-144.
[61] Paramvir Bahl and Venkata N. Padmanabhan, “RADAR: An In-Building RF-based User
Location and Tracking System” in Proceedings of IEEE Infocom 2000, Tel-Aviv, Israel, March
2000, pp 775-784.
[62] K. Yu and I. Oppermann, “Performance of UWB Position Estimation Based on TOA
Measurements”, in Proceedings of the Joint UWBST and IWUWBS, Kyoto, Japan, 2004, pp.
400-404.
[63] James F. O'Brien, Robert E. Bodenheimer, Gabriel J. Brostow, and Jessica K. Hodgins,
“Automatic Joint Parameter Estimation from Magnetic Motion Capture Data”, in Proceedings
of Graphics Interface, Montreal, Quebec, Canada, May 15-17, 2000, pp. 53-60.
[64] J. Hummel, M. Figl, C. Kollmann, and H. Bergmann, “Evaluation of a Miniature
Electromagnetic Position Tracker”, Medical Physics, Vol. 29, No. 10, October 2002, pp. 2205–
2212.
[65] Suzanne LaScalza, Jane Arico, and Richard Hughes, “Effect of Metal and Sampling Rate on
Accuracy of Flock of Birds Electromagnetic Tracking System”, Journal of Biomechanics, Vol.
36, No. 1, 2003, pp. 141–144.
[66] Nirupama Bulusu, John Heidemann, and Deborah Estrin. “GPS-less Low Cost Outdoor
Localization for Very Small Devices”, IEEE Personal Communications Magazine, Vol. 7, No.
5, Oct. 2000, pp. 28-34.
[67] A. Mason, A. Shaw, and A. I. Al-Shamma’a, “Intelligent Radio Frequency Identification
Positioning Using Wireless Sensor Networks”, IEEE Antenna and Propagation Conference,
Loughborough, UK, 2-3 April, 2007 pp. 145-148.
132
[68] Robert J. Fontana, “Recent Applications of Ultra Wideband Radar and Communications
Systems” in Ultra-Wideband, Short-Pulse Electromagnetics 5, Kluwer Academic/Plenum
Publishers, New York, 2002. pp. 225-234.
[69] Volker Schwarz, Alex Huber, and Michael Tüchler, “Accuracy of a Commercial UWB 3D
Location/Tracking System and its Impact on LT Application Scenarios”, IEEE international
Conference on Ultra-wideband, Sept 5-8, 2005, pp 599 – 603.
[70] P. Arcara and C. Melchiorri, “3D Position Measurement Based on Force Sensors for a OneWire Haptic Interface”, 16th IEEE Instrumentation and Measurement Technology Conference,
Venice, Italy, May 24-26, 1999, Vol. 2, pp. 1272-1277.
[71] “Space Sensor: 3D Sensing with Ease and Affordability”, Space Age Control Inc. [Online].
Available: http://www.spaceagecontrol.com/Main/3DDisplacementTransducer [Accessed Jan.
28, 2010].
[72] D. Lin, L. Keck Voon, H. Guo Rong, and N. Nagarajan, “GPS-based Attitude Determination
for Microsatellite Using Three-Antenna Technology”, in Proceedings of Aerospace Conference,
March 6-13, 2004, Vol. 2, pp. 1024-1029.
[73] Lu, G., M. E. Cannon and G. Lachapelle, “Attitude Determination in a Survey Launch Using
Multi-antenna GPS Technologies”, in Proceedings of the ION 1993 National Technical
Meeting, San Francisco, Jan. 20-22, 1993, pp. 251-260.
[74] E. Cattrysse, S. Provyn, P. Kool, O. Gagey, J. Clarys, P. and Van Roy, “Reproducibility of
Kinematic Motion Coupling Parameters During Manual Upper Cervical Axial Rotation
Mobilization: A 3-dimensional in Vitro Study of the Atlanto-axial Joint”, Journal of
Electromyography and Kinesiology, Vol. 19, No. 1, 2009, pp. 93-104.
[75] Nikolaos Strimpakos, Vasiliki Sakellari, Georgios Gioftsos, Eleni Kapreli, and Jacqueline
Oldham, “Cervical Joint Position Sense: an Intra- and Inter-examiner Reliability Study,” Gait
& Posture, Vol. 23, 2006, pp. 22-31.
[76] Wolf, R., G. W. Hein, B. Eissfeller, and Loehnert, “An Integrated Low Cost GPS/INS Attitude
Determination and Position Location System,” in Proceedings of ION GPS-96 Meeting,
Kansas City, Missouri, 1996, pp. 975–981.
[77] Eric Foxlin, Michael Harrington, and Yury Altshuler, “Miniature 6-DOF Inertial System for
Tracking HMDs”, in Proceedings of SPIE Vol. 3362, the AeroSense 1998 Conference on
Helmet- and Head-Mounted Displays III, Orlando, FL, 1998, pp. 214-228.
[78] Y. C. Ho and R. C. K. Lee, “A Bayesian Approach to Problems in Stochastic Estimation and
Control,” IEEE Trans. Automatic Control, Vol. 9, No. 4, Oct. 1964, pp. 333-339.
[79] D. Fox, J. Hightower, L. Liao, D. Schulz, G. Borriello, "Bayesian Filters for Location
Estimation", IEEE Pervasive Computing Magazine, 2003, p. 24-33.
[80] B. M. Scherzinger, J. J. Hutton, and J. C. McMillan, "Low Cost Inertial/GPS Integrated
Position and Orientation System for Marine Applications," IEEE Aerospace and Electronic
Systems Magazine, Vol. 22, No. 12, May 1997, pp. 15-19.
133
[81] A. Noureldin, R. Sharaf, A.H. Osman, and N. El-Sheimy, “INS/GPS Data Fusion Technique
Utilizing Radial Basis Functions Neural Networks” in Proceedings of the IEEE PLANS,
Monterey, CA, April 2004, pp. 280-284.
[82] K. Touil, M. Zribi, J. B. Choquel, and M. Benjelloun, “Bayesian Bootstrap Filter for integrated
GPS and Dead Reckoning Positioning”, IEEE, International Symposium on Industrial
Electronics, ISIE07, Spain, 2007, pp. 1520-1524.
[83] Ali Asadian, Behzad Moshiri, and Ali Khaki Sedigh, "A Novel Data Fusion Approach in an
Integrated GPS/INS System Using Adaptive Fuzzy Particle Filter", 5th International
Conference on Technology and Automation, Oct. 15-16, 2005, Thessaloniki, Greece. pp. 125130.
[84] Nguyen Ho Quoc Phuong, Hee-Jun Kang, Young-Soo Suh, Young-Shick Ro, and Kyu-Chan
Lee, “A GPS/INS Integration System with New Orientation Measurement Scheme for Land
Vehicle Application”, in Proceedings of the 32nd Annual Conference on IEEE Industrial
Electronics, Nov. 6-10, 2006, Paris, France, pp. 3099-3104.
[85] Pifu Zhang, Jason Gu, Evangelos E. Milios, and Peter Huynh, ``Navigation with
IMU/GPS/Digital Compass with Unscented Kalman Filter'', in Proceedings of IEEE
International Conference on Mechatronics and Automation, Jul. 29 – Aug. 1, 2005, Niagara
Falls, Ontario , Canada, pp. 1497 – 1502.
[86] E. Foxlin and L. Naimark “VIS-Tracker: A Wearable Vision-Inertial Self-Tracker”
Proceedings of IEEE Virtual Reality, Los Angeles, USA, March 22-26, 2003, pp. 199-206.
[87] L. Naimark and E. Foxlin, “Circular Data Matrix Fiducial System and Robust Image
Processing for a Wearable Vision-inertial self-tracker”, in Proceedings of IEEE International
Symposium on Mixed and Augmented Reality, Sep. 30 – Oct. 1, Darmstadt, Germany, 2002,
pp. 27–36.
[88] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, P. Nordlund,
"Particle Filters for Positioning, Navigation, and Tracking," IEEE Transactions on Signal
Processing, Vol. 50, No. 2, 2002, pp. 425-437.
[89] N. Yang, W. F. Tian, Z. H. Jin, and C. B. Zhang, “Particle Filter for Sensor Fusion in a Land
Vehicle Navigation System,” Measurement science and technology, Vol. 16, 2004, pp. 677-681.
[90] P-J. Nordlund and F. Gustafsson. “Sequential Monte Carlo Filtering Techniques Applied to
Integrated Navigation Systems”, in Proceedings of the 2001American Control Conference, Vol.
6, 2001, pp. 4375-4380.
[91] R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems”, Journal of
Basic Engineering, Vol. 82 D, 1960, pp. 35–45.
[92] S. Thrun, W, Burgard, and D. Fox, Probabilistic Robotics, MIT Press, Cambridge, MA, 2005.
[93] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A Tutorial on Particle Filters for
Online Nonlinear/non-Gaussian Bayesian Tracking”, IEEE Trans. on Signal Processing, Vol.
50, No. 2, February 2002, pp. 174-188.
134
[94] Changhe Yuan and Marek J. Druzdzel, “Theoretical Analysis and Practical Insights on
Importance Sampling in Bayesian Networks”, International Journal of Approx. Reasoning 46,
2007, pp. 320-333.
[95] Byoung-Tak Zhang, “A Bayesian Framework for Evolutionary Computation,” Proc. of the
1999 Congress on Evolutionary Computation, Washington DC. USA, Jul. 6 -9, 1999, pp. 722728.
[96] S. Park, J. Hwang, K. Rou, and E. Kim, "A New Particle Filter Inspired by Biological
Evolution: Genetic Filter”, in Proceedings of World Academy of Science, Engineering, and
Technology, Vol. 21, Jan., 2007, pp. 459-463.
[97] R. Fung and K. C. Chang, “Weighting and Integrating Evidence for Stochastic Simulation in
Bayesian Networks”, in Proc. of the Fifth Conference on Uncertainty in Artificial Intelligence,
Windsor, Ontario, Aug. 18-20, 1989, pp. 209-219.
[98] K. Kanazawa, D. Koller, and S. J. Russell, “Stochastic Simulation Algorithms for Dynamic
Probabilistic Networks,” in Proceedings of the Eleventh Annual Conference Uncertainty AI,
1995, pp. 346–351.
[99] Nelson F. Ford and Jarick Rager. “Expert System Support in the Textile Industry: End Product
Production Planning Decisions”, Expert Systems with Applications. Vol. 9, No. 2 1995, pp.
237-246.
[100] K. D. Schnelle and R. S. H. Mah, "A Real-time Expert System for Quality Control", IEEE
Expert, October 1992, pp. 36-42.
[101] Alexander Prusak and Axel R. Hidde, “The Use of Knowledge-based Systems for PCB Testing
and Repair”, Expert Systems with Applications, Vol. 5, No. 1-2, 1992, pp. 141-151.
[102] Spengler, M. Stanton, and M. Rowlands, “Expert Systems and Quality Tools for Quality
Improvement”, in Proceedings of Seventh IEEE International Conference on Emerging
Technologies and Automation, Vol. 2, October 19-21, 1999, Barcelona, Spain, pp. 955-962.
[103] L. A. Zadeh, “Fuzzy Sets”, Information and Control Vol. 8, 1965, pp. 338-353.
[104] L. A. Zadeh, “Fuzzy Algorithms”, Information and Control, Vol. 12, 1968, pp. 94-102.
[105] L. A. Zadeh, “Outline of a New Approach to the Analysis of Complex Systems and Decision
Processes”, IEEE Trans. on Systems, Man, and Cybernetics, Vol. SMC-3, No.1, 1973, pp. 2844.
[106] Steven R. Swanson, “A fuzzy Navigation State Estimator for GPS/INS Integration”, Plans
1998, Apr. 20-23, 1998, Houston, TX, USA, pp. 541-548.
[107] Walid Abdel-Hamid, Aboelmagd Noureldin, and Naser El-Sheimy “Adaptive Fuzzy Modeling
of Low Cost Inertial Based Positioning Errors”, Fuzzy Systems, Vol. 15, No. 3, 2007, pp. 519529.
[108] Hiliuta, R. Jr. Landry and F. Gagnon, “Fuzzy Correction in a GPS/INS Hybrid Navigation
System”, IEEE Trans. Aerospace and Electronic Systems, Vol. 40, No. 2, 2004, pp. 591-600.
135
[109] Immanuel Ashokaraj, Antonios Tsourdos, Peter Silson, Brian White, John Economou, “Feature
Based Robot Navigation: Using Fuzzy Logic and Interval Analysis”, FUZZ-IEEE July, 25-29,
2004 Budapest, Hungary pp. 1461-1466.
[110] “Canadian Gravity Standardization Network”, Natural Resources of Canada, [online].
Available: http://csrsjava.geod.nrcan.gc.ca/csrsjcpe/GSDreportsEN?user_name=CGIS2658&
querytype=CGSN&key=93231965 [Accessed: Oct. 2, 2008].
[111] A. G. Ledroz, E. Pecht, D. Cramer, and M. P. Mintchev, “FOG-Based Navigation in Downhole
Environment During Horizontal Drilling Utilizing a Complete Inertial Measurement Unit”,
IEEE Trans Instrumentation and Measurement, Vol. 54 No.5, Oct. 2005, pp. 1997-2006.
[112] Y. Suh, “Development of an INS Integrated Autonomous Positioning System for Assisting
Effective Fire-fighting Activity”, KSCE Journal of Civil Engineering, Vol. 8, No. 5, Sept. 2004,
pp. 567-574
[113] L. Ojeda and J. Borenstein, "Personal Dead-reckoning System for GPS-denied Environments",
IEEE International Workshop on Safety, Security, and Rescue Robotics (SSRR2007) in Rome,
Italy, September 27-29, 2007, pp. 1-6.
[114] Y. Suh, “Development of an INS Integrated Autonomous Positioning System for Assisting
Effective Fire-fighting Activity”, KSCE Journal of Civil Engineering, Vol. 8, No. 5, Sept. 2004,
pp. 567-574.
[115] J. C. K. Chou, “Quaternion Kinematic and Dynamic Differential Equations”, IEEE Trans.
Robot. Autom., Vol. 8, No. 1, Feb. 1992, pp. 53–64.
[116] D. H. Titterton and J. L. Weston, 1997, Strapdown Inertial Navigation Technology, Peter
Peregrinus Ltd. United Kingdom.
[117] J. Huddle, “Trends in Inertial Systems Technology for High Accuracy AUV Navigation”, In
Proceedings of the 1998 Workshop on Autonomous Underwater Vehicles MA, USA, Aug. 2021 1998, pp. 63-73.
[118] L.A. Zadeh, “Outline of a New Approach to the Analysis of Complex Systems and Decision
Processes”, IEEE Transaction on Systems, Man, and Cybernetics, Vol. SMC-3, No.1, 1973, pp.
28-44
[119] L. A. Zadeh, “Similarity Relations and Fuzzy Orderings”, Inform. Sci., Vol. 3, 1971, pp. 177200.
136