Upload
vancong
View
227
Download
0
Embed Size (px)
Citation preview
AB HELSINKI UNIVERSITY OF TECHNOLOGY
Department of Automation and Systems Technology
Farrukh Iqbal Sheikh
Real-Time Human Arm Motion Translationfor the WorkPartner Robot (Draft Version 1.0)
Thesis submitted in partial fulfillment of the requirements for the degree
of Master of Science in Technology
Espoo June 25, 2008
Supervisors:
Professor Aarne Halme Professor Kalevi Hyyppä
Helsinki University of Technology Luleå University of Technology
Instructor:
Research Scientist Sami Terho
Helsinki University of Technology
HELSINKI UNIVERSITY ABSTRACT OF THE
OF TECHNOLOGY MASTER’S THESIS
ii
Author: Farrukh Iqbal Sheikh
Title of the thesis: Real-Time Humar Arm Motion
Translation for WorkPartner Robot
Date: June 25, 2008 Number of pages: 87
Department: Automation & System Technology
Professorship: e.g. Automation Technology Code: e.g. AS-84
Supervisor: Prof Dr. Aarne Halme
Instructor: Research Scientist. Sami Terho
In response of ever increasing demand of intelligent robot in our society, the
natural ways of human robot interaction have been investigated in terms of
speech, vision and physical interfaces. As the speech and vision methods require
power processing and intensive calibration which makes them hard to implement.
Thus, the physical interfaces are still in use. These interfaces are improving the
cooperative behavior of man and machine.
The aim of thesis is to design and investigate the use of emerging motion capture
technique for the future robot. In thesis the human arm motion has been utilized
to control the human like manipulator of the robot in real-time dynamic task
environment. This technique offers great benefit in advance teleoperation and
robotic control through motion learning. Keeping main objective in mind the
various techniques of motion capture is reviewed. Based on reliability, accuracy
and real-time performance the inertial based MoCAP technique is selected. The
approach is validated by the construction of low cost 3D orientation sensor includ-
ing miniature accelerometer, gyroscope and magnetometer. The accuracy of the
3D orientation measurement has been enhanced by the implementation of sensor
fusion based real-time extended Kalman filter. The 3D orientation performance
using extended Kalman Filter is tested and verified. In the following an idea
of sensor sleeve comprises using four 3D orientation sensors is presented which
allow the user to control the human like robot arm directly using his arm motion.
Further, the virtual 3D kinematic simulator of the WorkPartner body included
manipulator is developed which was required to prevent damaging the real robot.
Finally the result of real-time human arm motion translation is presented and
briefly discussed.
Keywords: Motion Capture MoCAP, 3D orientation sensor,
Extended Kalman Filter, WorkPartner Simulator, Real-time motion conversion.
iii
TEKNILLINEN DIPLOMITYÖN
KORKEAKOULU TIIVISTELMÄ
Tekijä: Etunimi Sukunimi
Työn aihe: Työn otsikko
... voi olla pitkä
Päivämäärä: 25. kesäkuuta 2008 Sivumäärä: 87
Osasto: Osastosi
Professuuri: esim. Automaatiotekniikka Koodi: esim. AS-84
Työn valvoja: Etunimi Sukunimi
Työn ohjaaja: Etunimi Sukunimi
Suomennettu tiivistelmä tänne.
Avainsanat: avainsana-1, avainsana-2.
iv
Contents
1 Introduction 1
1.0.1 Thesis Objective . . . . . . . . . . . . . . . . . . . . 3
1.0.2 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . 4
1.1 Introduction of Space Mission Scenario . . . . . . . . . . . . 5
2 Literature Review 7
2.1 History of Biomechanics . . . . . . . . . . . . . . . . . . . . 8
2.2 Human Motion Capture (MoCAP) Systems . . . . . . . . . 10
2.2.1 Magnetic MoCAP . . . . . . . . . . . . . . . . . . . . 13
2.2.2 Acoustic MoCAP . . . . . . . . . . . . . . . . . . . . 14
2.2.3 Inertial And Magnetic MoCAP . . . . . . . . . . . . 15
2.3 Use of MoCAP in Robotics . . . . . . . . . . . . . . . . . . . 16
2.4 The WorkPartner Robot . . . . . . . . . . . . . . . . . . . . 18
2.4.1 Specification of the WorkPartner . . . . . . . . . . . 19
2.4.2 TorsoController . . . . . . . . . . . . . . . . . . . . . 20
2.5 Measurement Constrain in Human Arm Model . . . . . . . . 22
2.5.1 Gesture Recognition using Accelerometer . . . . . . . 24
3 Sensors for Angle Measurement 28
3.1 Working Principle of Inertial and Magnetic Sensor . . . . . . 29
3.1.1 Accelerometer . . . . . . . . . . . . . . . . . . . . . . 29
3.1.2 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . 31
3.1.3 Magnetometer . . . . . . . . . . . . . . . . . . . . . . 33
3.1.4 Bend Sensor . . . . . . . . . . . . . . . . . . . . . . . 35
v
3.2 Performance Evaluation of Angle Sensor . . . . . . . . . . . 36
3.2.1 Hardware Setup . . . . . . . . . . . . . . . . . . . . . 36
3.2.2 Inclination Angle using Inertial Measurement Unit
(IMU) . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.2.3 Heading Angle using Magnetometer . . . . . . . . . . 40
4 3D Orientation Measurement Using Inertial And Mag-
netic Sensor 44
4.1 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.1.1 Accelerometer Measurement Model . . . . . . . . . . 48
4.1.2 Gyroscope Measurement Model . . . . . . . . . . . . 49
4.1.3 Magnetometer Measurement Model . . . . . . . . . . 49
4.2 Filter Structure . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.2.1 Quaternion System State Model . . . . . . . . . . . . 51
4.2.2 Measurement Model . . . . . . . . . . . . . . . . . . 54
4.2.3 Update State Covariance . . . . . . . . . . . . . . . . 59
4.2.4 Error Covariance . . . . . . . . . . . . . . . . . . . . 60
4.3 Experiement . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
4.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
4.4.1 Case 0: Influence of Environment Noises . . . . . . . 64
4.4.2 Case 1: Influence of the external magnetic disturbance 66
4.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
5 Kinematic Analysis & Modeling of WorkPartner Manip-
ulator 72
5.1 Specification of WorkPartner Manipulator . . . . . . . . . . 74
5.2 Kinematic Modeling of WP Manipulator . . . . . . . . . . . 75
5.2.1 WorkEnvelop . . . . . . . . . . . . . . . . . . . . . . 76
5.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . 78
5.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
6 Results 79
vi
7 Summary and Conclusions 80
References 81
A Calibration of Orientation Sensor Module 88
A.1 Experiment 1: Determination of Qk and Rk . . . . . . . . . 90
B Name of the 2nd appendix 97
vii
List of Figures
2.1 Davinci Human Arm Drawing . . . . . . . . . . . . . . . . . 9
2.2 a)Borelli’s Observation (Toulmin, 2001),b)Human motion anal-
ysis (Etenne Marey) . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Gypsy ElectroMechanical MoCAP (Gypsy6, 2008) . . . . . . 12
2.4 Optical MoCAP for Animating "Lord of the Ring"character
Gollum (Yu, 2007) . . . . . . . . . . . . . . . . . . . . . . . 13
2.5 Magnetic MoCAP, Magnetic Field Transmitter located (James F
and Hodgins, 2000) . . . . . . . . . . . . . . . . . . . . . . . 14
2.6 Inertial and Magnetic Based MoCAP (Xsens, 2008) . . . . . 16
2.7 NASA Robonaut, Hybrid robot designed to use in future
space mission (Moreno, 2007) . . . . . . . . . . . . . . . . . 18
2.8 Artistic and Real WorkPartner Robot (TKK, 2008) . . . . . 19
2.9 Kinematic Model of WorkPartner Robot (TKK, 2008) . . . . 20
2.10 Torso Controller for WorkPartner (Suomela, 2004) . . . . . 21
2.11 Mechanical Model of Human Arm, bottom right part indi-
cate shoulder joint (Karim Abdel-Malek, 2003) . . . . . . . 23
2.12 Physical segment model of attached sensor frame, angle mea-
surement of joint segment using inertial sensor (Rong Zhu,
2004) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.13 Accelerometer configuration for elbow angle measurement (Satoshi KU-
RATA, 1999) . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.14 principle of shoulder angle measurement (Satoshi KURATA,
1999) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
viii
3.1 Three cases of single axis Accelerometer,A single axis ac-
celerometer mass suspended by the string, displacement of
mass from it position of equilibrium measure the accelera-
tion exert by the force,+ve and −ve indicates expansion and
compression . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.2 The configuration of tri Axes Accelerometer using single axis
accelerometers. . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.3 Practical example of Coriolis AccelerationMovement of per-
son on the rotating platform exhibit coriolis acceleration which
is proportional to angular velocity of the frame (John Geen,
2008). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.4 Inner structure of Analog Devices iMEMs Gyroscope (John Geen,
2008). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
3.5 Principle of Hall Effect and Magneto resistive Magnetometer 34
3.6 Flexible bend Sensor (Sensors, 2008) . . . . . . . . . . . . . 35
3.7 Data Acquisition Hardware,Composed of single microcon-
troller with multiple SPI and ADC port for Inertial, magnetic
and bend sensor, two USART for RX,TX data communication. 37
3.8 Analog ADIS16350 Inertial Measurement Unit, Composed
of tri Axes accelerometer, Gyro and temperature sensor . . . 38
3.9 Inclination Measurement using IMU Tri Axes Accelerometer,
First Graph indicates the vector component, Second Graph
is the measured of corresponding inclination angle . . . . . . 39
3.10 Inclination Measurement using of IMU Tri Axes Gyros, Ob-
served angular rate of each axis of gyro vector . . . . . . . . 40
3.11 Integrated tri axis Gyro rotations, Roll, Pitch and Yaw Angles 41
3.12 Heading Measurement using Tri Axes Magnetometer, Top
graph is the measurement of magnetic vector, middle graph
is heading angle output and bottom is the magnetic loop
calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
ix
4.1 Structure of Sensor Fusion, Accelerometer As , Magnetome-
ter Ms and Gyroscope GS are combined to estimate quater-
nion based orientation, ZA and ZM are the calibrated sensor
outputs respectively, Qm is measurement quaternion obtained
by ZA and ZM , the estimated error in Qm is corrected with
the fusion of calibrated gyro ZG using Kalman filter based on
noise covariance Rk and Qk. . . . . . . . . . . . . . . . . . . 47
4.2 Magnetic inclination Angle,H is the projection of the F mag-
netic field vector on XY Plane of Earth surface, I is dip angle
by the surface. . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.3 Process Flow of Quaternion Based Extended Kalman Filter,
P (k|k) is state covariance, Q(k) is measurement Covariance,
R(k) is System Covariance, w(k) represents Measurement
Noise, z(k) represents observations equivalent to measure-
ment quaternion Qm , v(k) represents additional System Noise,
u(k) is gyro rate input equivalent to ZG and q(k) is the quater-
nion state vector to be estimated by the predictive process of
extended Kalman filter, q, z are state and measurement pre-
diction respectively and v(k + 1) measurement residual. . . . 52
4.4 Orientation in Earth Fixed Frame . . . . . . . . . . . . . . . 55
4.5 3D Orientation Sensor Module, Module is depicted in var-
ious views with their corresponding sensor reference frame
is indicated by the x(Red) ,y(Black) and Z( Green) at the
bottom corner . . . . . . . . . . . . . . . . . . . . . . . . . . 62
4.6 Orientation sensor rotation about Roll φ , Pitch θ and Yaw
ψ, anticlockwise about an each axis is +ve deg and clockwise
is -ve deg. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
4.7 30min Tri Axis Acceleration, Gryroscope and Magnetometer
observation. . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.8 3D Euler orientation measurement at rest. . . . . . . . . . . 66
x
4.9 Histogram comparison of 3D orientation with and without
Kalman estimator. . . . . . . . . . . . . . . . . . . . . . . . 67
4.10 Tri Axis Magnetic Vector disturbance due to ferromagnetic
electric iron. . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4.11 Yaw angle measurement with and without Kalman filter un-
der magnetic disturbance. . . . . . . . . . . . . . . . . . . . 68
4.12 BoxPlot of Yaw measurement with and without Kalman. . . 69
5.1 3D CAD model of WorkPartner Robot . . . . . . . . . . . . 73
5.2 Joint Specification of WorkPartner Manipulator . . . . . . . 74
5.3 DH Kinematic Model of WP Manipulator . . . . . . . . . . 76
5.4 WorkEnvelop of Single WP Manipulator,a)Left Side view, b)
Top viewShoulder motion range is -45 deg-45 deg in tilt and
-90 deg-90 deg in inclination, Elbow motion is in 0 deg-140
deg inclination followed by shoulder motion, Wrist motion is
-90 deg-90 deg in both inclination and rotation followed by
the shoulder and elbow motion. . . . . . . . . . . . . . . . . 77
5.5 3D Simulator for the WorkPartner . . . . . . . . . . . . . . . 78
A.1 Orientation sensor Modules,Left one module is developed by
Xsens Technologies, Right one is Self developed using ADIS16350
(Analog Devices) and HM55B (Hitachi). . . . . . . . . . . . 89
A.2 Tri Axis Accelerometer,Gyroscope and Magnetometer mea-
surement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
A.3 Histogram fit of Accelerometer,Gyroscope and Magnetometer. 92
A.4 Histogram fit of Quaternion Measurement Vector Components 93
A.5 Quaternion Observation using Discrete Integration Algorithm 94
A.6 Histogram fit of Quaternion Vector Components using Gyro
Angular rate. . . . . . . . . . . . . . . . . . . . . . . . . . . 95
A.7 Tri Axis Temperature Observation (30min) . . . . . . . . . . 96
xi
Symbols and Abbreviations
Ak,i[β] angle difference function
Bk,in,m matrix element correlation function
symbol explanation
HMI Human Robot Interaction
MoCAP Motion Capture
WP WorkPartner Robot
IMU Inertial Measurement Unit
xii
Chapter 1
Introduction
A constantly increasing the growth of complex autonomous and teleoper-
ated robots are becoming widespread in our society. A significant progress
made in the fields of Electronic, Mechanical and Computer Science are
equally contributing to introduce sophisticated artificial algorithms and in-
novative sensors and actuators designs that making robots more dynamic
and humanoid. In the same time, increase in processing power giving them
freedom to process complex autonomous algorithms for indoor and outdoor
robots. However, all the advancement and achievement to make robots ar-
tificially intelligent and capable to adopt environmental changes still far
from the present. Therefore, Human robot interaction is necessary up to
certain extent.
The Human Robot Interaction (HMI) is the field in which various methods
have been researched to develop the various schemes of interaction between
human and robots. Traditionally some of the techniques are utilizing nat-
ural way of interaction such as speech recognition, symbolic and vision
systems. These techniques require more processing and complex procedure
2
which makes them slow on today’s processor. Thus, the new techniques
have been researched in which human motion could be utilized to control
the robot for autonomous and non autonomous tasks. It is also known as
gesture recognition.
In last decade, the scope of human motion was limited for the application of
animating virtual character, biomedical etc. Nowadays the newly developed
motion capture techniques are being considered for the robotics application,
especially for the tele-operation of robotic arm. Today’s the concept of
humanoid and hybrid robots are popular. They have complete and semi
mechanical structure inspired by the human skeleton. Research made on
these robots are planned to be use in Space Planetary exploration and
rehabilitation of patient etc. The main idea of utilizing human arm motion
is to enhance the capability of such robots in complex task environment
through teleoperation and real-time direct operation. The WorkPartner
hybrid structured field service robot (TKK, 2008) is also being developed
to provide help to the user in complex task environment through natural
way of human interaction. The proposed researched is conducted for the
WorkPartner Robot. Using this approach semi autonomous feature of the
WorkPatner will be augmented which enables the user to control the motion
of manipulator of robot such that a specific object could be picked and
grab. While in autonomous mode, it could be used for motion teaching of
the robot. It will increase the ability of robot to adopt new motion through
motion learning mechanism. The advantage of using this approach is not
limited for field and service robots but it can be directly applied for future
humanoid robot and human like 7Dof robotics arm.
3
1.0.1 Thesis Objective
The focus of the thesis is on the real time translation of the human arm
motion sequences into the equivalent motion of human like manipulator of
the robots. In order to accomplish the main goal of the thesis work, it has
been structured in two main challenges. Each challenge is comprises on sub
problem that led to the specific goal of that challenge. The task hierarchy
is presented as follows.
1. Motion Capture technique of Human Arm Sequence
• Research and analyzed of traditionally developed various human
motion capture techniques.
• Study the possible measurement constrain.
• Develop a low cost motion sensor.
• Design and implement the sensor fusion based estimation algo-
rithm.
• Evaluate the performance of orientation sensor module for real-
time application.
• Record joint motion sequence.
1. Translation of real-time human arm motion sequence
• Researched and analyzed mode of operation of the WorkPartner
Manipulator.
• Analyzed motion translation constrains.
• Design and implement the 3D Kinematic Simulator of Work-
Partner Manipulator.
• Design and develop the central Graphical User Interface (GUI)
for real-time motion translation.
4
1.0.2 Thesis Outline
The Thesis report is organized in to the following chapters.
Chapter 1 provides the general overview of the thesis and possible chal-
lenges including space mission scenario designed for the use of MoCAP in
future planetary space exploration is discussed.
Chapter 2 the primitive motion capture technique followed by the history of
biomechanics and their use in various multidisciplinary fields are discussed.
Further the specification of the WorkPartner Robot is illustrated in con-
text of thesis. While in last the possible measurement constrain in Human
arm model in case of positioning sensor for an accurate joint measurement
based on inertial and low cost accelerometers are discussed.
Chapter 3 covers the basic principle of miniature inertial (accelerometer
and gyroscope), magnetometer and bend sensor. Based on their working
principle each sensor is evaluated for angle measurement. Finally, the effect
of noises present in each sensor is briefly described with the help of practical
experiment using developed data acquisition hardware and sensor circuitry.
Chapter 4 the effect of disturbance and noises in inertial (Accelerometer
and Gyroscope) and Magnetometer are mathematically modeled. Based on
their measurement state models, the Sensor Fusion based quaternion ex-
tended Kalman filter technique for an accurate complete 3d orientation
measurement using inertial (accelerometer and Gyroscope) and magne-
tometer is presented. Further the estimation filter design aspects such
as Measurement model, System state model, Update covariance and error
1.1 Introduction of Space Mission Scenario 5
covariance is covered. Finally, the performance of the filter is evaluated by
an experiments and their results are discussed.
1.1 Introduction of Space Mission Scenario
The motivation of this thesis is to provide the use of human motion cap-
ture technique for the future space mission. The typical scenario can be
imagined, when the human and robot will land together on the surface of
Mars for the exploration of life.
To define the mission, we need to make some assumption. These assump-
tions are as follows.
• Robot is WorkPartner, which has human like manipulator of 7 DOF
(Degree of Freedom).
• Astronaut suit which has some additional micro motion sensing ca-
pability using MEMS sensors.
• Portable micro computer is installed on the left arm of Astronaut
suit.
• The space camp has also provided by the space ship on the Mars.
• Solar panel is installed at the space camp.
Now, the mission is ready to imagine. Under all these assumption provided.
When the WorkPartner robot with semi autonomous mode (autonomous
and non autonomous) capability will starts to explore the field of Mars.
During the exploration the human astronaut will receive its moves, simul-
taneously by means of wireless link that works within 1Km range between
1.1 Introduction of Space Mission Scenario 6
astronaut computer and the robot (Martin, 1999).
First, the human astronaut starts the robot GUI on his computer, through
which human will interact with robot. This GUI is composed of all neces-
sary features of tele-robotics and navigational control etc. Now robot has
received the command by the astronaut to explore the field within circle
of 900m. Robot started and exploring the field. In the mean time during
exploration, human found some useful objects that can help to proof the
existence of human life. Here, the human Astronaut has taken the control
by switching the operational mode. After getting the control, he indicates
the object of interest by highlighting the object in GUI on his computer.
At this point the human astronaut has two choices one is to leave on the
robot to pick that object autonomously and other is to use the motion
translation mechanism for the precise and accurate teleoperation. The de-
cision of the choice is depends on the nature of object. If the object seems
to be sensitive and small then human motion translation would be the wise
choice. Otherwise autonomous operation would be better.
If we suppose that robot has some artificial intelligence capability like,
learning from experience. Still the human motion can be used to provide the
fastest and easiest way of teaching the robot about task specific motions.
Chapter 2
Literature Review
In order to develop the better understanding of proposed thesis work, re-
view of multi disciplinary works are necessary. Therefore, some relevant
papers and research articles from the fields of Biomechanics, Computer
Sciences, Robotic and Electronics have been reviewed. As the nature of
proposed researched is based on the utilization of human motion technique
for the future robot which can be categorized into the field of bio-mechanic
and robotics. Thus, the following sections covered the introduction of the
evolution of the field of bio-mechanics concerning human anatomy, studied
of various state of the art technologies of motion capture and their mea-
surement constrain in respect of real-time motion translation for the robot.
Further, the recent use of motion capture technique in the field of space
robotics is discussed. At last the possible constrain in motion measurement
technique using inertial and low cost miniature accelerometer is presented
followed by the WorkPartner specification.
2.1 History of Biomechanics 8
2.1 History of Biomechanics
In early centuries the human motion analysis was unimagined and rigorous
to comprehend. By the passage of time when the history of science com-
menced with primeval Greeks, who were first inquired human attributes to
comprehend the nature in nexus of our perception. As Socrates the great
scientist of Greek’s time taught that "we could not understand the world
unless we not able to understand our own Nature" (Martin, 1999). After his
execution, the intellectual concepts of Socrates had profound effect on his
student Plato. Plato was the first, who introduced basic theories of philol-
ogy and Mathematics concerning nature. Aristotle (384 BC - 322 BC),
the great physician went to the Athens and studied at Plato’s academy.
He had extraordinary capabilities of observing anatomy of living things as
mechanical system. The book "De Motu Animalium" has written by Aris-
totle was the proof of his advancement made in Biomechanics. Certainly,
he could be considered the first biomechanics. He was the author of book
called "De Motu Animalium"- which was based on the analysis of animal
anatomy (Martin, 1999).
After contribution of these philosophers the rebirth of biomechanics had
evolved once again due to the work made by the great Artist Lenoardo da
Vinci (1453-1519).He was the famous artist whose work has judged as an
engineer. He had the better understanding of today’s terminology of forces
vector, friction coefficient and the acceleration of falling object exact be-
fore their evolution (leonardo-da-vinci biography, 2008). He analyzed the
insight attributes of human structure such as muscles forces, joint motion
for his artwork as depicted in figure 2.1.
Later, the father of mechanics and modern science Galileo Galilee (1564-
1643) who structured the early studied of biomechanics mathematically.
2.1 History of Biomechanics 9
Figure 2.1: Davinci Human Arm Drawing
The progress made by Galileo, Borelli (1608-1679) able to define the force
required for equilibrium in various joints of human body earlier than New-
ton’s 3 laws of motion as shown in Figure 2. Perhaps, He was the first who
defined the center of gravity position of human structure. The work and ef-
fort made by these pioneer was followed by "Newton (1642-1727),Bernoulli
(1700-1783),Euler (1707-1783), Poiseuille (1799-1869), Young (1773-1829)
and other equal fame" (Schneck, 2000). Hence, the evolution of biome-
chanics and human motion analysis was started. First practical studied
in this field for human motion analysis was carried by the Etenne Marey
(1830 - 1904) through the use of image sequences as shown in 2.2.
Figure 2.2: a)Borelli’s Observation (Toulmin, 2001),b)Human motion anal-
ysis (Etenne Marey)
2.2 Human MoCAP Systems 10
2.2 Human MoCAP Systems
Motion capture MoCAP is the recording of human body poses or other
movement for immediate or delayed analysis and playback. The informa-
tion captured can be as general as the simple position of the body in space
or as complex as the deformations of the face and muscle masses.
In 20th century, there are many advancement have made in this field by the
multidisciplinary scientists who make them dream of capturing human mo-
tion live for certain application true. The main intention of motion capture
was studying the human motion and its kinematics which revealed diverse
results in the form of kinematics and inverse kinematic models, such as
minimum-torque-change model and Donders’s law etc. These studies were
based on posture based methods as described in (Marjan A. Admiraal
and Gielen, 2004). Now these studies are also helping the scientists to find
better understanding of human anatomy like muscle contractions about
articulating joint movement, based on the theory other aspects of human
motor control system and gait dynamics is also being improved. Currently,
the use of human motion capture has revolutionized the "Entertainment in-
dustry" by animating virtual character for games and 3D animated movies
for example Tomb raider (game), Beowulf etc. Probably, the use of motion
capture for computer character animation is relatively new. Because the
requirement of human motion capture system for animating virtual char-
acter in movies had realized in late 1970 after the great cartoon movie
Pinocchio and Snow white produced by Walt Disney, which was based on
"rotoscoping" technique (Johnston, 1981). In this technique artist sketch
the cartoon character behavior directly over the video sequence of human
performer. Besides this the use of human motion have also been providing
an evident helped in medical in such a way that disabilities of patient after
an accident could be improved by analyzing human walking pattern and
2.2 Human MoCAP Systems 11
motion. This is also known as rehabilitation. Usually these analysis helped
athletes in sports. Now the use of real-time human motion capture system
with some accuracy can be realized in robotics. In such a way, that any
part of the robot, which is similar to the part of human body, could be used
to control the robot. This realization can be used for the motion teaching
and advance telemetry operation for the robots.
There are various techniques of capturing human poses have developed so
far, which one is suitable in context of the thesis is evaluated by reviewing
earlier developed motion capture systems. In gerneral they can be catago-
rized as follows.
1. Electro Mechanical MoCAP
2. Optical MoCAP
3. Magnetic MoCAP
4. Acoustic MoCAP
5. Inertial and Magnetic MoCAP
Electro Mechanical MoCAP
It is one of the earliest developed methods for capturing human motion.
In this technique the combination of on/off mechanical switch and com-
plex motion tracking systems is used. The designed is based on a set of
armatures that are attached all over the performer’s body as shown in fig-
ure 2.3.In this approach armatures are connected to each other by using
a series of rotational and linear encoders. These encoders are wired to an
interface that can be simultaneously read all the encoders in order to pre-
vent data skewing. However, this technique provide clean rotational data
2.2 Human MoCAP Systems 12
that can be collected in real time without any occlusion problems. Finally,
through a set of trigonometry functions, the performer’s motion are ana-
lyzed and recorded. The main disadvantage is the design restrictions which
seem to be quite difficult to overcome, and will probably limit the use of
these type of devices for character animation (Wes Trager, 1999).
Figure 2.3: Gypsy ElectroMechanical MoCAP (Gypsy6, 2008)
Optical MoCAP
Optical MoCAP is developed in late 80’s for capture human poses. It is
currently used in character animation of movies as shown in figure 2.4.
The principle on which it is working is based on triangulation method by
camera at the orthogonal position. There are two ways of implementing
an optical MoCAP. One is by placing passive reflectors markers (retro-
reflective material), on the human joint position that reflects existing light
present in the environment and second is by using Active Reflector marker
such as LED that blinks in timely manner.
Optical MoCAP utilizes proprietary video cameras to track the motion of
reflective markers (pulsed LED’s) attached to joints of the actor’s body.
Single or dual camera systems are suitable for facial capture, while 3 to 16
or more camera systems are necessary for full-body capture. Reflective op-
tical MoCAP uses Infra-red (IR) LED’s mounted around the camera lens,
along with IR pass filters placed over the camera lens. Optical motion cap-
ture systems based on Pulsed-LED’s measure the Infra-red light emitted
2.2 Human MoCAP Systems 13
Figure 2.4: Optical MoCAP for Animating "Lord of the Ring" character
Gollum (Yu, 2007)
by the LED’s rather than light reflected from markers (Wes Trager, 1999).
This system suffer due to the occlusion (Line of Sight) problem. The noise
due to the occlusion is compenstated by the virtual rigid human human
skeleton for enhancing the accuracy of the Optical MoCAP (L. Herda and
Thalmann, 2000).The main disadvantage of this system is computationally
expensive,cost factor and large space requirement for the operation which
is not suitable for real-time and portable application.
2.2.1 Magnetic MoCAP
In Magnetic MoCAP the tri axis magnetic sensor are strapped on the hu-
man joints that measure the change in direction of magnetic field generated
by the transmitting source as shown in figure 2.5 . In this approach change
in vector direction provides the measure of 3D position and orientation of
human Joint in calibrated workspace. Finally, the resulting data stream
is usually applied in inverse kinematic model to animate the human skele-
ton. The main intent of this technique was to avoid the problem due to
occlusion problem as presented in optical MoCAP. However, the sense
magnetic field decreases as distance increases by the transmitter and can
2.2 Human MoCAP Systems 14
be affected by the interference of addition magnetic field and ferromagnetic
material nearby. Therefore, these dependencies limit the use of system in
fixed calibrated environment. Hence, it cannot be applied for the robotics
application but it is feasible for teleoperation of robotic arm (James F and
Hodgins, 2000).
Figure 2.5: Magnetic MoCAP, Magnetic Field Transmitter lo-
cated (James F and Hodgins, 2000)
2.2.2 Acoustic MoCAP
Acoustic MoCAP is another method has been used for recording human
motion. In this technique the triad low cost audio receivers are located at
distance and arrays of audio transmitters are strapped to various joint parts
of the human body is used. These transmitters are sequentially triggered
to produces encoded sound signal and each receiver measures the time of
flight of the transmitted signal. Thus, the calculated distance of the three
receivers is triangulated to measure a point motion in 3D space. An inher-
ent issue with this approach is the sequential nature of the position data
which require additional processing. This position data is typically applied
to an inverse kinematics system that drives an animated skeleton.
One of the big advantages of this method is the lack of occlusion problems
normally associated with optical systems. However, the several negative
2.2 Human MoCAP Systems 15
factors associated with this method made this approach complex. First,
there is the fact that the cables can be a hindrance to various types of
performances which can be avoided with use of wireless data transmitter.
Second, the limitation of number of sensors cannot be increased to capture
the completed motion sequences. Third is the size of the capture area,
which is limited by the speed of sound in air and the number of transmitters.
In addition, the accuracy of this approach can sometimes be affected by
spurious sound interference.
2.2.3 Inertial And Magnetic MoCAP
The motion capture technique based on miniature inertial and magnetic
sensor is recently revealed in the last few years. This system is currently
being used by Xsens Technology (Xsens, 2008). In this technique the combi-
nation of Inertial (gyroscope and accelerometer) and magnetic sensor is uti-
lized to measure the orientation of human joint. The MEMS (Micro Elector
Mechanical Sensor) technology made this approach feasible for human mo-
tion application. In this approach the ambulatory tracking of human poses
is implemented by fusing the tri axis gyro, tri axis accelerometer and tri axis
magnetic sensor data using complementary Kalman filter (Daniel Roeten-
berg and Veltink, 2007a; Rong Zhu, 2004).
A complementary Kalman filter that operated on error model rather than
system state model presented in (Daniel Roetenberg and Veltink, 2007a) is
used to estimate the complete 3D orientation. As the gyroscope measures
the angular velocity which can be integrated in time to obtain the orienta-
tion of the sensor module. However the small gyro drift caused the large in-
tegration error in time measurement. In order to compensate the gyro drift,
the absolute orientation reference is provided by the use of accelerometers
and magnetometers. In this approach the magnetic sensor measurement
2.3 Use of MoCAP in Robotics 16
could be affected by the Ferromagnetic materials nearby. This problem
can be categorized into two types, one as hard disturbance and other as
soft disturbance. The hard disturbance is due to the magnet object present
in the environment. It can be eliminated by means of system calibration
but the soft disturbance affect by the Ferros material nearby has been
overcome by the use of Kalman-based fusion algorithm (Daniel Roetenberg
and Veltink, 2007a).The typical coniguration of multiple inertial sensor on
human skeleton is indicated in figure 2.6.
Figure 2.6: Inertial and Magnetic Based MoCAP (Xsens, 2008)
However the use of Inertial and Magnetic motion capture allow us to ac-
curate implement the real-time ambulatory tracking of human posture and
movement for the robotics application. Although, there are some measure-
ment error still associated in this approach but it can be rectified further
by fusing redundant measurement through goniometry and optical MoCAP
measurement data.
2.3 Use of MoCAP in Robotics
Motion capture system can be applied in various robotics applications as
stated earlier, such as virtual robotics control, task teaching for humanoid
2.3 Use of MoCAP in Robotics 17
robot, entertainment, bio medical and telerobotics. The aim of this thesis
work is to implement the motion capture of human arm for the manipula-
tor of work partner robot. The advantage of capturing human arm motion
sequences is because of unique motion sequences that allow human to do
routinely task efficiently. This motion can be possibly applied for any hu-
man like robotic arm of 7 DOF.
The task teaching through learning and telerobotic are the potential ap-
plications of motion capture systems. Recently, the advancement made
in integrated MEMS (Micor Electro Mechanical System) sensors and com-
puter vision algorithm made this technique possible for the field human
robot interaction. As the work presented in article (Yasuyoshi YOKOKO-
HJI, 2002), in which author has proposed a stereo vision based motion
learning method for the robot. In this approach the humanoid robot is
used to adopt the human motion sequence directly from the work envi-
ronment. First the robot records the human motion sequence and then it
patterns the sequence such that the motion could be learned and reused.
Although it is the more natural way of interacting with robot but due to
the complexity of vision algorithm and dynamic task environment it is hard
to processed in real-time.
In similar way, the NASA is utilizing the inertial sensor based motion cap-
ture techniques to control the movement of Astronaut robot called Robo-
naut as presented in (Miller, 2004). In the presented work the motion
technique is utilized for the teleoperation of the robonaut. It is designed
to remotely control the robonaut in such a way the problem occurred in
Space Station could be fixed without intervention of human astronaut. The
typical view of robonaut is shown in figure 2.7.
2.4 The WorkPartner Robot 18
Figure 2.7: NASA Robonaut, Hybrid robot designed to use in future space
mission (Moreno, 2007)
2.4 The WorkPartner Robot
A mobile and service robot is being designed to work interactively with hu-
mans in urban environment by using natural communication means. The
development of such capabilities in the robots is one of the biggest chal-
lenges of future intelligent machines. The basic design attributes of service
robots are light weight structure, mobile, flexible and ability to adapt the
task motion and environmental changes through learning interface. Based
on some of these attributes the robot ASIMO (ASIMO, 2008) has been
developing but most of the features are still under developed. The design
of robot ASIMO is inspired by the human structure and its motion, such
robot are also called "Humanoid ROBOT".
By considering the future of intelligent service robot, the Department of
Automation and System technology of Helsinki University of Technology,
Finland have been developing the service robot called Workpartner, since
decade as shown in figure 2.8 . It is designed to perform daily routine tasks
through interaction with the people in the urban environment. Some of
interaction has been done in terms of speech recognition, symbolic repre-
sentation and human wearable mechanical gesture recognition device (Torso
2.4 The WorkPartner Robot 19
controller). Further techniques of interaction are being researched for en-
hancing intelligence.
Figure 2.8: Artistic and Real WorkPartner Robot (TKK, 2008)
2.4.1 Specification of the WorkPartner
The complete structure of the work partner robot is hybrid. It comprises
on four legs with active body joint wheels that enhance the mobility of
the robot in narrow areas, while the wheel allow the robot to travel fast
enough on uneven terrain. The actuation system is electrical powered by
the means of batteries and the combustion engine generator. It can attain
maximum speed of 7 Km/h on hard terrain using wheel motion as pre-
sented in (Aarne Halme and Kettunen, 2003).
As can be seen in figure 2.8, the front part of the robot is comprise on two
human arms like manipulator, body and vision cameras installed at head
position. The Segments of arm are revolute joined by the rotating actu-
ators. These rotating actuators are simple DC motors with tailor made
planetary gears. The brake in each acutators allow the user to manullay
positioned the robotic manipulator.
2.4 The WorkPartner Robot 20
The Robot WorkPartner single side manipulator is comprises on 5 revolute
joints as shown in figure 2.9. In which, 2 are used for wrist motion (one
is for inclination and one is for rotation), one is for elbow and 2 are used
to represent shoulder motion (one is for inclination and one is for yaw
rotation). All joints actuators are controlled by their dedicated controller.
The controller is connected with the main embedded board running on
QNX Embedded operating system through CAN (Controller Area Network)
bus. The function of embedded board is to decode the receive commands
received by the wireless link and generate the appropriate control signals
for the sub controllers.
Figure 2.9: Kinematic Model of WorkPartner Robot (TKK, 2008)
2.4.2 TorsoController
In order to control the robot manipulator motion for task teaching by the
use of direct teleoperation the "Torso Controller" was developed (Suomela,
2004). The torso controller was designed to controls the WP manipula-
tor directly by the operators arm movement. It can be categorized into
electromechnical motion capture system. In torso controller techniques op-
erator wear the elecrto mechanical controller suit on his shoulders as shown
in Figure 8. The controller suit is comprises of inertial sensor, which mea-
sure bend angle of human body using accelerometer and gyroscope, while
2.4 The WorkPartner Robot 21
the position of human arm was carried out by the 2DOF gimbaled wire
potentiometer.
Figure 2.10: Torso Controller for WorkPartner (Suomela, 2004)
However, it is low cost solution which was specifically designed for the ma-
nipulators of WorkPartner but it is not feasible for the complete translation
of human arm motion which is comprises on 7DOF. The main disadvan-
tage of this approach is the use of wired potentiometer whose rotation is
tethered by the joystick which must be grabbed to control the manipulator
of the robot. Therefore, it can only be used to point the target object but
whenever the operator want to grab the object is not possible in natural
way using his palm and wrist fingers motion.
2.5 Measurement Constrain in Human Arm Model 22
2.5 Measurement Constrain in Human Arm
Model
The anatomy of human arm is the real complex entity. In order to do
the real-time translation of human arm motion for the robot then the un-
derstanding of the human arm anatomy is equally important as capturing
human arm posture. In general, the upper limb is composed of three me-
chanical like joints shoulder, Elbow and wrist, their combination allow to
move arm possibly in any direction. Due to the complex anatomy of human
arm, it has highest mobile part of the human body. The typical attached
reference frame on each joint in human arm can be seen in figure 2.11. In
total, the DOF of human arm are 9. If we simplify the DOF by ignoring
the joint motion q1 and q2, which is relatively less mobile as compare to
other joints then the total DOF will reduce to 7. It can be confirmed by
the Figure 10. In which q3, q4 and q5 are representing the joint motion
of shoulder, where we can place the reference frame for the translation of
motion. While the elbow joint can be seen as q6 that directly link with
the motion of shoulder joint. In the end, the wrist joint with 3 DOF q7,
q8 and q9 whose motion links with elbow and shoulder according to the
rigid body mechanism. Further detailed is presented in (W. Maurel, 2004;
Karim Abdel-Malek, 2003).
The knowledge of human arm anatomy helps to define the position of
sensors that could provide an accurate measurement of joint orientation.
There are two types of problem associated with human arm motion due to
anatomy. First there is no rotary mechanical type joint in between the two
segments of human arm. Secondly the flexion in human joint during motion,
which could results in small translation of segment in body attached sensor
frame but its magnitude is small enough and could be ignored and assumed
no translation in sensor attached frame. These measurement problems has
been considered and simplified with the use of kinematic and trigonometric
2.5 Measurement Constrain in Human Arm Model 23
Figure 2.11: Mechanical Model of Human Arm, bottom right part indicate
shoulder joint (Karim Abdel-Malek, 2003)
theory as discussed in (Rong Zhu, 2004).
Figure 2.12: Physical segment model of attached sensor frame, angle mea-
surement of joint segment using inertial sensor (Rong Zhu, 2004)
According to kinematic theory, the orientation of human joint like elbow
can be determined using motion data acquired by the sensors (tri axis in-
ertial plus magnetic sensor module) positioned on segments as shown in
figure 2.12. As can be seen in Figure 11, there are two sensors frames at-
tached to the segments respectively. One sensor frame is represented by
frame i , while other one is by frame i+1 .
2.5 Measurement Constrain in Human Arm Model 24
The respective attached tri-axis 3d motion sensor including accelerometer,
gyroscope and magnetometer on the segment provide the measurement
in sensor frame. Each sub sensors measure its corresponding x, y and
z component in sensor coordinate that has been combined using Kalman
Filter. It measures the orientation in sensor frame. As indicated earlier the
two sensors have been used, one is on upper segment and the other is on
lower segments. Finally the rotation around the elbow joint denoted by θ,
has been computed using rotation matrix as shown in (2-1).where Ki,i+1
= [ Kx, Ky,Kz], in frame Xi,Yi and Zi Versθ = 1- cosθ 2.1.
Rot(Ki+1
i , θ) =
K2xV ersθ + cos θ KxKyV ersθ −Kz sin θ KxKzV ersθ +Ky sin(θ)
KxKyV ersθ +Kz sin θ K2yV ersθ + cos θ KyKzV ersθ −Kx sin θ
KxKzV ersθ −Ky sin θ KyKzV ersθ +Kx sin θ K2yV ersθ + cos θ
(2.1)
The result using this approach shows that the two 3d motion sensors on
connected segment could be accurately used to measure the rotation about
the joint. The Author in (Rong Zhu, 2004) has tested the same principle
on the human arm to measure elbow joint rotation by positioning sensor
on upper and lower limb of human arm. It shows that the same method is
applicable to record human arm motion sequence.
2.5.1 Gesture Recognition using Accelerometer
The cost effective solution for upper limb joint motion recognition is also
possible by the use of low cost tri axes accelerometer as presented in (Satoshi KU-
RATA, 1999). In this method the author states that if two tri axes ac-
celerometers are positioned near to the joint then the difference of these
sensors output produces by the rotation around the joint can be used to
estimate the angle between the limbs.
2.5 Measurement Constrain in Human Arm Model 25
The author has tested his method on elbow like joint that produces rotation
about one axis as shown in figure 2.13.
Figure 2.13: Accelerometer configuration for elbow angle measure-
ment (Satoshi KURATA, 1999)
In this approach the elbow motion has measured by the tri axes accelerom-
eter 1 and 2 both are positioned near to the actual joint motion. According
to the author the accuracy of method depends on how close these two sen-
sors are from the joint. This configuration is named by the author as "Both
near Sides" as presented in (Satoshi KURATA, 1999).
By the practical, the different amount of loaded acceleration a on ac-
celerometer 1 and acceleration a’ on accelerometer 2 due to the rotation of
rigid shoulder joint has observed. Using x and y component of acceleration
of accelerometer 1 (ax1, ay1), and 2 (ax2,ay2), the angle between two limbs
around elbow joint as rotation matrix is described as shown in 2.2.
(
ax1
ay1
)
=
(
cos(θ) − sin(θ)
sin(θ) cos(θ)
) (
ax2
ay2
)
(2.2)
Thus, the joint angle θ can be described by 2.3.
tan(θ) =ax2, ay1 − ax1.ay2
ax1.ax2 − ay1.ay2
(2.3)
2.5 Measurement Constrain in Human Arm Model 26
This similar approach has also tested for the shoulder joint. In this case
the different configuration of two tri axes accelerometers mounted near to
shoulder joint is used as shown in figure 2.14.
Figure 2.14: principle of shoulder angle measurement (Satoshi KURATA,
1999)
In this configuration the acceleration a and a’ of two tri axis accelerometers
on the respective positions has combined with the Euler rotation matrix as
shown in equation 2.4.
ax1
ay1
az1
= Rxyz
ax2
ay2
az2
(2.4)
As stated earlier shoulder joint is complex, therefore the acceleration of
frames a and a’ cannot give the measure of orientation. This problem has
been resolved by the use of gravitational component of respective frame as
shown in equation 2.5.
gx1
gy1
gz1
= Rxyz
gx2
gy2
gz2
(2.5)
2.5 Measurement Constrain in Human Arm Model 27
Using equation 2.5, the equation 2.6 and 2.7 has been derived to obtain
the rotation angles.
tan(α) =
√
(sin(Ψ)2 + (tan θ)2
cos(θ)(2.6)
tan(β) =sin(Ψ)
tan(θ)(2.7)
However, when the subject is either moving fast or at rest the equation 2.6
and 2.7 will no longer be applicable as discussed (Satoshi KURATA, 1999).
This indicates that the user cannot freely obtained its motion sequence for
the application of robotics but proposed method is suitable to implement
a low cost solution for only the elbow joint.
Conclusively, the most appropriate approach according to the requirements
of the thesis is leads to inertial and magnetic sensor motion capture scheme.
The advantages and portability of motion capture using inertial and mag-
netic sensor package with implementation of estimation algorithms can pro-
vide better results for the real-time translation of human motion sequences
to the manipulator of Robot.
Chapter 3
Sensors for Angle Measurement
Sensors are the most commonly use devices in any application. They are
used to convert the physical quantity such as acceleration, velocity, distance
etc into proportional electrical signals. This section describes the absolute
detail of sub sensors of orientation sensor module with the help of their
basic principle and numerous practical tests. The results and knowledge
obtained by the following tests are discussed in the following section. There
are two types of sensors, some sensors are based on the principle of MEMS
technology (Accelerometer and Gyroscope) and other sensors are based on
material deformation properties and basic electromagnetic law for measur-
ing physical quantity, such as the Earth’s magnetic field.
The low cost MEMS (Micro Electro Mechanical System) inertial sensors
have been widely used in variety of applications for measuring acceleration
and angular motion. The intention of developing the low cost MEMS is to
provide the valuable low cost solution for various applications, where cost,
size and power consumption are major concerned. A typical use of inertial
sensor can be seen in cruise attitude control, robotics navigation and also in
3.1 Working Principle of Inertial and Magnetic Sensor 29
today’s smart phone interfaces that allows user to interact with the future
mobile device in unconventional way. According to these applications it can
also be used to measure the orientation of human joint poses as presented
in (Stilson, 1996).
3.1 Working Principle of Inertial and Mag-
netic Sensor
3.1.1 Accelerometer
A single axis accelerometer measures the acceleration along sensitive axis.
It contains a mass suspended by the spring in sensor housing as shown in
figure 3.1. The mass is only allowed to displace in one direction by the
effect of force and acceleration due to gravity. The displacement of mass
in sensitive direction measures the acceleration (a) and direction of gravity
vector (g) in presence of environment noises.
The physical principal on which it is working is known as Hooke’s law.
Hooke’s law of elasticity states that the amount of deformation (compress
or expand) in term of displacement of mass attached by the spring from its
equilibrium position is linearly proportional to the force. Mathematically,
it can be expressed as 3.1.
F = −Kx (3.1)
Where, K is spring constant , F is the force exert on the material and x is
the distance of compressed and stretched from the equilibrium position or
the position of mass at zero force. Another important physical law called
Newton’s second law of motion which states that force on mass is directly
3.1 Working Principle of Inertial and Magnetic Sensor 30
x = 0
M Se n s i tive
Ax is
M Se n s i tiv e
Ax is
x < 0 -v e
M Se n s i tive
Ax is
x > 0 + ve
Figure 3.1: Three cases of single axis Accelerometer,A single axis accelerom-
eter mass suspended by the string, displacement of mass from it position of
equilibrium measure the acceleration exert by the force,+ve and −ve indi-
cates expansion and compression
proportional to acceleration, if object mass remains constant. Mathemati-
cally it can be expressed as 3.2.
F = ma (3.2)
Where, m is the mass and constant of proportionality, ’a’ is the acceleration
caused by the force. Hence, by comparing these two physical laws governed
the relation of acceleration caused by the force in terms of displacement of
mass as (a = -k x/m). If we able to observe the displacement of mass con-
nected to a spring then acceleration can be measured. There are various
methods of sensing change in displacement which also defines the type of
sensor. One of popular type of sensing change is by the capacitance [2].
This physically phenomenon is fabricated in small housing for measure ac-
celeration caused by the force. In order to measure the acceleration in 3
axes the same single axis accelerometer can be duplicated at tri axes or-
thogonal positions, where each axis is 90 degree a part from its neighboring
axis as shown in figure 3.2.
Typical applications where the accelerometer could be utilized are incli-
3.1 Working Principle of Inertial and Magnetic Sensor 31
Figure 3.2: The configuration of tri Axes Accelerometer using single axis
accelerometers.
nation or tilt angle measurement, sensing amount of linear acceleration
of moving object (Inertial Measurement) and Vibration Measurement etc
(James, 2007). In all these applications the most commonly use is the in-
clination measurement that also being utilized in the low cost orientation
measurements of human joint posse as discussed in (Satoshi KURATA,
1999).
3.1.2 Gyroscope
Gyroscope is also known as rate sensor which is used to measure the an-
gular rate. There are different types of gyros available such as laser gyro,
spinning motor gyro, and piezoelectric based vibrating mass gyro[6].In all
these types, the vibrating mass is being widely used in iMEMS due to
small in size and low power consumption. Therefore, it is ideally suited for
human motion analysis. In today’s integrated MEMS technology measure
angular rate by means of Coriolis Acceleration (John Geen, 2008). The
term coriolis acceleration had been introduced by the French mathemati-
cian Gaspard G. de Coriolis, 1792-1843. It can be described as, consider
person is standing on the rotating platform, near at the center as depicted
3.1 Working Principle of Inertial and Magnetic Sensor 32
in figure 3. The relative tangential speed of person on the rotating disk
relative to the ground as shown by the blue arrow will increase in magni-
tude when person moves from center of rotating platform toward the outer
edge of the platform. This rate of change in tangential velocity is known
as Coriolis acceleration (John Geen, 2008).
Figure 3.3: Practical example of Coriolis AccelerationMovement of person
on the rotating platform exhibit coriolis acceleration which is proportional
to angular velocity of the frame (John Geen, 2008).
In practical the deflection of frame containing resonating mass produce
coriolis acceleration.The spring is used to attach the resonant mass with the
substrate. It is also called Coriolis sense fringes (John Geen, 2008)as shown
in figure 4. These fringes are capacitively coupled to sense the displacement
of the frame in reaction of the force generated by the resonating mass.
Figure 3.4: Inner structure of Analog Devices iMEMs Gyro-
scope (John Geen, 2008).
The force caused by the coriolis acceleration is also known as coriolis force.
3.1 Working Principle of Inertial and Magnetic Sensor 33
By using Newton’s second law of motion the coriolis acceleration as the
function of angular velocity can be expressed as 3.3.
A = 2 ∗ V ∗W (3.3)
Where A is coriolis acceleration, V is mass speed and W is the angular
velocity of the rotating platform. Using this equation the angular velocity
of rotating platform can be obtained by the coriolis acceleration. Same as
3d Accelerometer the 3 axes gyroscope configuration can be designed.
3.1.3 Magnetometer
Magnetometer sensor is used to measures the strength of magnetic field.
There are many type of magnetometer sensor available, such as mechani-
cal, Fluxgate, magneto inductive, magneto resistive and Hall Effect mag-
netometer (Everett, 1995). Among all these the Hall effect and magneto
resistive are popular due to easy of sensing , low power consumption and
small in size. A single axis Hall Effect sensor work on the principle called
Hall Effect. It states that, if the electric current flows through the conduct-
ing plate in a magnetic field then the magnetic flux exert a transverse force
on mobile charges which tends to generate the potential across the plates
called Hall voltage as shown in figure 3.5. The amplitude of hall voltage
gives measures of the magnetic field strength (Magnet.fsu, 2008). While
in case of Magneto resistive the change in resistance due to the magnetic
field is used in wheat stone bridge configuration to measure magnetic field
strength (Stefan Hübschmann, 1996)as shown in figure 3.5.
One advantage of hall method is the inherent ability to directly sense the
strength of magnetic field. Mathematically hall voltage is given by 3.4.
Vh =(− IB
d)
ne(3.4)
3.1 Working Principle of Inertial and Magnetic Sensor 34
I (current)
V = VH (Hall Voltage)
V =0
B (Magnetic Flux)
w
d
R
R
R
R
BV v
Magnetic
Resistance
B( Magnetic Flux)
Figure 3.5: Principle of Hall Effect and Magneto resistive Magnetometer
Where Vh, Hall voltage, I , biased current through the electric plate, n is
the charge density and e is charge of electron. The 2D orthogonal configu-
ration of single magnetic sensor can be used to measure the heading or the
angle from magnetic north. This configuration is also known as magnetic
compass. Heading measurement or angle from magnetic north is simply
obtained by the trigonometric relation between two axis magnetic strength
as given by 3.5.
θ = arctan
(
By
Bx
)
(3.5)
Where,θ- Heading angle. By - Magnetic Field strength at y. Bx- Magnetic
Field Strength at x.
The following terms are associated with the magnetic sensor. "Declination
(D)" is the angular difference between the heading of true north and mag-
netic north. "Inclination (I)" is the angle above or below the horizontal.
"Total strength (T)" or Magnitude of vector magnetic field that measure
of magnetic field strength nearby (nationalatlas, 2008).
Magnetometer is common for heading measurement using the Earth’s mag-
netic field, especially for navigation of ship, car and unmanned aircraft etc.
The same property of magnetometer has been utilized with accelerometer
for the 3D attitude estimation as presented in (Egziabher, 2008).
3.1 Working Principle of Inertial and Magnetic Sensor 35
3.1.4 Bend Sensor
It is the unique sensor that is used to measure the bend angles. It utilizes
the material deformation properties that produce change in resistance, at
the time of sensor bends (Sensors, 2008). The typical resistance based
bend sensor is shown in figure 3.6. As shown that it gives the resistance
of 10Kohm in un-flexed state and in bend state the resistance increases
gradually. It can attains the max resistance in range of 30-40Kohm at the
90 deg.
Figure 3.6: Flexible bend Sensor (Sensors, 2008)
Typical use of such sensor can be seen in virtual gloves, where it uses to
measure the bend of human hand fingers. However, it requires additional
signal conditioning circuitry and calibration for measuring change in resis-
tant proportional to the bend angle.
3.2 Performance Evaluation of Angle Sensor 36
3.2 Performance Evaluation of Angle Sensor
The Performance measurement is critical and necessary step in sensor eval-
uation. It gives the idea of how to use these sensors properly and accurately
in our application. It also provides evident result based on which further
enhancement and modification can be possible.
3.2.1 Hardware Setup
The data acquisition hardware is constructed to measure the performance
of accelerometer, gyroscope, and magnetometer for the application of an
angle measurement. It is comprises on a single AT90CAN128 Atmel u
controller, power circuitry and several sensors connectors as shown in fig-
ure 3.7. In general all sensors are operated on SPI (Serial Peripheral Inter-
face) protocol, but each of them has its own sets of command and protocol
configuration through which it can be configured and calibrated. This con-
figuration makes them hard to program using single controller. Despite the
main controller only have a single SPI (Serial Peripheral interface) port,
several virtual SPI ports has been created programmatically. As presented
in [ref] on single SPI interface more than one device can be operated in
master slave configuration but for fast and reliable data acquisition each
sensor is programmed through dedicated its single SPI ports as shown in
figure 3.7.
3.2.2 Inclination Angle using IMU
The ADIS16350 Inertial Measurement unit (Inertial Measurement Unit)
that contains tri axes accelerometer, gyro and temperature in a single pack-
age were tested as shown in 3.8.
3.2 Performance Evaluation of Angle Sensor 37
USART 0
USART 1
AT90CAN128
u Controller
SPI 2
SPI 1
ADC 1
ADIS16350
IMU
(3 Accel, 3 Gyro)
HM 55B
Magnetic sensor
Bend Sensor
Figure 3.7: Data Acquisition Hardware,Composed of single microcontroller
with multiple SPI and ADC port for Inertial, magnetic and bend sensor,
two USART for RX,TX data communication.
In performance tests the IMU is rotated (0deg - 90deg and 90deg - 0deg)about
X axis (roll). The Inclination angle formed by the X-Y plane from the Earth
surface has observed through the use of accelerometer and Gyroscope. In
case of accelerometer measurement, the change in earth gravity vector sense
by the tri axis accelerometer is used for inclination angle. Therefore, un-
wanted linear and angular acceleration has to be digitally low passed filtered
using either standard Butterworth low pass filter or the normal average fil-
ter. Before filtering the measurement data the bias factor must be obtained
using standard calibration. These steps must be followed for smooth and
accurate measurement. The measurement data has recorded by the sensor
are fully calibrated and filtered from unwanted acceleration component as
depicted in first graph of figure 3.9.
As shown in figure 3.9, the rotation about the x axis only affects the y-
z gravitational component as function of trigonometric sine. Thus, the
simple mathematical relation as indicated in 3.6 can be used to measure
3.2 Performance Evaluation of Angle Sensor 38
Figure 3.8: Analog ADIS16350 Inertial Measurement Unit, Composed of
tri Axes accelerometer, Gyro and temperature sensor
the inclination angle.
θ = arcsin
(
YAccel + bias
9.8
)
(3.6)
Where, θ - Inclination angle roll. YAccel - Low pass filtered y component of
acceleration vector. Bias - Calibrated bias factor.
The corresponding inclination angle obtained by the measurement is de-
picted in second graph of figure 3.9. It has been verified by the use of
fourth order curve fitting polynomial as indicated as the green line on the
graph. It represents the ideal response that can be used as a reference for
performance measurement. As can be seen in figure 3.9, the fluctuation
due to additional linear acceleration was occurred during 130 -135 sample
which limit the use of single accelerometer for an accurate angle measure-
ment.
Under the same condition, the IMU gyroscope was tested. The calibrated
data acquired by the tri axes is recorded as shown in figure 3.10. As can
be clearly seen in figure 3.10, how much the noise and bias drift is domi-
nant on the real measurement. It can be compensate by obtaining proper
3.2 Performance Evaluation of Angle Sensor 39
0 50 100 150 200 250 300−2
0
2
4
6
8
10Tri Axes Acceleration (m/s2)
T Sample
Acc
eler
atio
n (m
/sec
2 )
0 50 100 150 200 250 300−20
0
20
40
60
80
100Inclination Measurement (
T Sample
Ang
le (
deg)
Accel XAccel YAccel Z
Inclination angle Curve Fitting
Figure 3.9: Inclination Measurement using IMU Tri Axes Accelerometer,
First Graph indicates the vector component, Second Graph is the measured
of corresponding inclination angle
bias factor through calibration. In another way, it could be minimized at
start by enabling self-test function as described in ADIS1625 calibration
procedure (DEVICES, 2008).
However, this problem cannot be limit to zero. As the gyroscope give the
angular rate which can be integrated to obtain the angular displacement.
In same measurement condition, when IMU was rotated about x axis. The
integrated angular displacement along each axis is computed using discrete
integration as shown in figure 3.11. The left most graph represent the roll
angles, which indicates the rotation about the X axis (roll). It is also an
inclination angle in this case. While the other graphs shows the rotation
about y (pitch) and z (yaw). The result of continuous short time integra-
tion due to drift is also indicated as the increase in pitch and yaw angles.
3.2 Performance Evaluation of Angle Sensor 40
0 50 100 150 200 250 300−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6Tri Axes Gyro (deg/s)
T Sample
Ang
ular
vel
ocity
deg
/sec
Gyro XGyro YGyro Z
Figure 3.10: Inclination Measurement using of IMU Tri Axes Gyros, Ob-
served angular rate of each axis of gyro vector
Another problem associated in orientation measurement using tri axis gyro
is the measure of angle without aiding. Therefore, initial reference must
be known whenever they require for the orientation measurement. One
advantage of using gyro is high operational bandwidth which is essentially
required for high speed motion sequences which cannot be obtained with
the use of accelerometer and magnetometer.
3.2.3 Heading Angle using Magnetometer
In magnetometer performance test the two HM55B Magnetic compass are
used to measure complete earth magnetic field vector. These two magnetic
compasses are orthogonally positioned to cover complete 3D magnetic field.
These sensors are subjected to measure the Earth’s magnetic field vector
3.2 Performance Evaluation of Angle Sensor 41
0 100 200 300−20
0
20
40
60
80
100Roll Angle using X Gyro rate
T Sample
Rol
l Ang
le (
deg)
0 100 200 300−2
0
2
4
6
8
10
12
14
16
18Pitch Angle using X Gyro rate
T Sample
Pitc
h A
ngle
(de
g)
0 100 200 300−18
−16
−14
−12
−10
−8
−6
−4
−2
0
2Yaw Angle using Z Gyro rate
T Sample
Yaw
Ang
le (
deg)
Figure 3.11: Integrated tri axis Gyro rotations, Roll, Pitch and Yaw Angles
but it is also sensitive to the other magnetic disturbances nearby. These
disturbances are caused by the interference of additional magnetic fields.
They are mainly categories as hard iron and soft iron disturbances. The
ferromagnetic material in surrounding causes hard iron or static distur-
bances. It can be reduced through calibration. While the soft disturbances
or dynamic distortion is occurred by the presence of Electromagnetic wave
that cannot be easily compensated. Therefore, calibration must be the
necessary measure of an accurate measurement. 3.12.
The heading angle has been observed by the test. During the test, tri axis
magnetic sensor has rotated clockwise and anti clockwise about z axis in
such a way that z axis remains aligned to the normal of the Earth surface.
In this case, heading angle could be obtained by the x, y component of
magnetic vector. The measurement data has recorded by the experiment
as shown in figure 3.12. The first graphs in figure 11 shows the behavior of
3.2 Performance Evaluation of Angle Sensor 42
0 100 200 300 400 500 600
−50
0
50
Tri Axes Magnetometer (uT)
T Sample
Mag
netic
Fie
ld u
T
Magno XMagno YMagno Z
0 100 200 300 400 500 600−200
−100
0
100
200Heading (Magnetic North)
T sample
Ang
le (
deg)
Heading Angle
−20 −15 −10 −5 0 5 10 15 20−20
−10
0
10
20Magnetoic Loop (uT)
X Magnetic Field uT
Y M
agne
tic F
ield
uT
Magnotometer XY loop
Magnetic Disturbance
Ideal response
Figure 3.12: Heading Measurement using Tri Axes Magnetometer, Top
graph is the measurement of magnetic vector, middle graph is heading
angle output and bottom is the magnetic loop calibration
magnetic field vector. While, the corresponding heading measurements are
depicted in the second of figure 3.12. The third graph of figure 11 which is
mainly used for the calibration. It shows that how much the environment
is suitable for heading angle measurement. As we know the strength of
magnetic field is equally spread everywhere. In ideal case when there is no
influence of magnetic disturbance. The magnetic loop should be appeared
in the shape of circle whose center at origin. However, due to the presence
of magnetic disturbances, it appeared in the shape of ellipse whose center
is also shifted as shown in figure 3.12.
The Performance test of each sensor concludes that the single sensor could
not be enough for an accurate orientation measurement. Therefore, sensor
3.2 Performance Evaluation of Angle Sensor 43
fusion based estimation algorithm is necessary to overcome the affect of
noises presented in each sensor, as discussed in following section.
Chapter 4
3D Orientation Measurement
Using Inertial And Magnetic
Sensor
All the advancement made in MEMS technologies enables micro machined
accelerometer and gyroscopes suitable for various application that includes
analysis of Human movement through recording (Eric Foxlin, 2004), Imag-
ing Platform stabilization control using counter rotation (Marcelo C. Al-
grain, 1993), Airborne Attitude control (Marmion, 2006), inertial naviga-
tion (David H. Titterton, 2004), mixed and augment reality (Eric R. Bach
and McGhee, 2003) and detecting levels of activity for rehabilitation of pa-
tients (REBELLO, 2004; Peter H. Veltink, 1996). In all these application,
they are mainly uses for orientation estimation. There are some limitations
still present due to substantial noise in sensors.
The orientation can be estimated by integrating the tri axis angular rate.
The MEMS Gyroscope can accurately be used to measure the angular rate
45
that can give the measure of orientation. However, a relatively small offset
error due to temperature effect, improper bias factor and additional mea-
surement noise may introduce large integration errors. Another problem of
using gyroscope is the measurement without aiding. Thus, initial position
must be known as aiding.
A tri axis linear accelerometer measures the sum of linear acceleration,
gravity and measurement noise. In absence of acceleration component as
in most case of human motion, the gravity vector can be used to measure
the inclination angle in Earth Fixed coordinate as presented in (Luinge and
Veltink, 2004). This property of accelerometer can be applied to compen-
sate the gyro drift for orientation measurement. Based on this feature the
gyroscope and accelerometer measurement has been fused to estimate ori-
entation of human segment using Kalman filter (Demoz Gebre-Egziabher
and Powell, 1998). The results indicate that they can only reduce the gyro
drift in 2DOF.
Since the accelerometer cannot be used to detect the rotation about ver-
tical axis (yaw). Thus, the yaw rotation must be included for complete
3d orientation. This problem has been resolved by the use of Magne-
tometer. It measures the heading angle from magnetic north. This prop-
erty of magnetometer has utilized together with the fusion of gyroscope
and accelerometer to measure complete 3D orientation (Eric R. Bach and
McGhee, 2003; Ashutosh Saxena and ebastien Ourselin, 2004; Eric R. Bach-
mann, 2001; Daniel Roetenberg and Veltink, 2007b). A gyro free orienta-
tion measurement method has also proposed (Demox Gebre-Egziabher and
W.Parkinson, 1998) to keep the system cost low but its performance indi-
cates that it can only be used to track slow motion but it is suitable for
navigational application. Still the affect of magnetic disturbances nearby
can affect the accuracy of overall orientation measurement, especially in
4.1 Sensor Fusion 46
heading angles ψ as discribed earlier. However, in control environment
such as in laboratory it can be characterized prior to the measurement,
which can be eliminated using calibration. Another method has also pro-
posed in which the GPS (Global positioning System) signals has been fused
instead of magnetometer to improve the accuracy of orientation estimation
as presented in (Demoz Gebre-Egziabher and Powell, 1998).The inherent
noise and error in GPS can affect the measurement but its performance
are considerable much better than magnetometer, especially in presence of
magnetic disturbance. However, it limits the use of orientation measure-
ment only in the Earth focused applications.
In this study, the various types of Kalman based fusion methods for ori-
entation measurement (Angelo M, 2006; Xiaoping Yun and B.McGhee,
2003; Kraft, 2003; Tatsuya Harada and Sato, 2003, 2007) have been stud-
ied. Based on the real-time performance the quaternion based standard
Extended Kalman filter technique is implemented for orientation measure-
ment. This chapter is focused on the design of quaternion based estimation
filter and its performance evaluation in the presence of static magnetic dis-
turbances and measurement noises.sd
4.1 Sensor Fusion
A Standard discrete non- linear Extended Kalman Filter was designed and
implemented to estimate an accurate orientation measurement. It com-
bines the complete three tri axis sensors information including tri axis ac-
celerometer, Gyroscope and magnetometer to estimate quaternion vector
of complete 3D orientation. The structure of estimation filter procedure
is shown in figure 4.1.In practical, the discrete Kalman Filter is used to
combine the feature of multi-sensors to estimate an accurate quantity of
4.1 Sensor Fusion 47
interest from the noisy measurement. As in this case, the noise present in
gyro as in form of drift is compensated with the fusion of accelerometer
and magnetometer.
LPF
LPF
Accelerometer
Model
Gyroscope
Model
Magnetometer
Model
Quaternion Based
Extended Kalman Filter
Rk Qk
Accelerometer
Signal As
Magnetometer
Signal Ms
Gyroscope
Signal Gs
Gs
Mf
ZA
ZM
ZG
QM
QuaternionOutput
System
Cov
Measurement
Cov
QO
Figure 4.1: Structure of Sensor Fusion, Accelerometer As , Magnetometer
Ms and Gyroscope GS are combined to estimate quaternion based orien-
tation, ZA and ZM are the calibrated sensor outputs respectively, Qm is
measurement quaternion obtained by ZA and ZM , the estimated error in
Qm is corrected with the fusion of calibrated gyro ZG using Kalman filter
based on noise covariance Rk and Qk.
In the applied fusion technique, the gravitational component Gs is obtained
by the Accelerometer signal As using low pass filtered. It reduces the af-
fect of additional acceleration component on inclination measurement. In
similar way, the Magnetometer Ms signal is also filtered to minimized the
immediate effect of high frequency magnetic disturbance due to mobile
signals, wireless signals in surrounding. After applying pre signal filtering,
the output signal GS and Ms are further processed through their respective
measurement model. It measures the calibrated measurements ZA and ZM .
Further, the ZA and ZM are combined together using kinematic of the earth
fixed coordinate system to form the measurement quaternion vector Qm.
The measurement quaternion contains the information about an absolute
reference in Earth fixed coordinate system. This information of an absolute
4.1 Sensor Fusion 48
reference is fused with gyro measurement by the use of quaternion system
state model as illustrated in preceding sections. This approach maintains
high dynamic response and increase accuracy of complete orientation mea-
surement. The Kalman filter combine all the sensors measurements and
estimate the correct measurement using system Rk and measurement noise
covariance Qk. These co variances are computed separately using sensor
calibration procedure. It uses noise covariance to predict the correct esti-
mate by reducing estimated state error. Hence, the noise free quaternion
vector output is estimated.
The sensors state model are the important factor to designed and implement
the estimation algorithm. Therefore, the following sensors measurement
models are discussed as follows.
4.1.1 Accelerometer Measurement Model
Accelerometer signal can be modeled as the sum of linear acceleration a,
magnitude of the gravity g and measurement white noise w as shown in
equation 4.1.
As(t) = a(t) − g(t) − w(t) (4.1)
In above equation, the amount of linear acceleration a(t) component can be
further modeled as the first ordered - low pass filtered white noise process
as expressed in equation 4.2.
a(t) = caa(t− 1) + v(t) (4.2)
Where, ca - low pass cut off frequency.
It can be experimentally determined for pre filtering process.
4.1 Sensor Fusion 49
4.1.2 Gyroscope Measurement Model
Gyroscope signal Gs can be modeled as the sum of angular velocity ω, offset
b and white noise w as indicated in equation 4.3.
Gs(t) = ω(t) + b(t) + w(t) (4.3)
The fluctuation of offset bt due to variation in temperature and misalign-
ment in gyro measurement can be described as the Markov process, driven
by the Gaussian white noise as shown in 4.4 .
b(t) = b(t− 1) + w(t) (4.4)
This factor must be obtained experimentally and subtracted from the read
gyroscope signal, Otherwise it increases as the amount of unwanted inte-
gration offset.
4.1.3 Magnetometer Measurement Model
Magnetometer signal can be modeled as the sum of the Earth magnetic
field me, Magnetic disturbance md and the uncorrelated Gaussian white
noise w as shown in 4.5.
Ms(t) = me(t) +md(t) + w(t) (4.5)
The Magnetic Disturbances md can be modeled as shown in equation 4.6.
md(t) = cdmd(t− 1) + w(t) (4.6)
Where, cd - constant range (0-1). w(t) is Gaussian white noise with stan-
dard with standard deviation δd. The presence of magnetic disturbance
4.1 Sensor Fusion 50
is detected by the magnitude of the Earth magnetic field vector.It can be
computed using 4.7.
MA =√
M2x +M2
y +M2z (4.7)
It indicates the existence of magnetic disturbances in surrounding. In case
of zero magnetic disturbances this magnitude of vector MA would be equal
to 1. Otherwise the measure of magnetic dip angle would be needed to
estimate the amount of magnetic disturbance in the Earth fixed coordinate.
The magnetic dip angle, also known as magnetic inclination.It is the mea-
sure of angle between magnetic field direction and surface of the Earth as
shown in figure 4.2. It gradually decreases from 90 deg at magnetic poles
to 0 deg at magnetic equator.
Figure 4.2: Magnetic inclination Angle,H is the projection of the F magnetic
field vector on XY Plane of Earth surface, I is dip angle by the surface.
Mathematically, it can be written as (3-8).
θ = arctan
(
Mz√
M2x +M2
y
)
(4.8)
4.2 Filter Structure 51
As shown in the formula the change in magnetic flux directly affect the
dip angle measurement. Thus, the amount of magnetic disturbance is ob-
tained by the comparison of measured dip angle with the ideal value of
dip angle at the location. Usually the disturbance is represented as the
vector of standard deviation ?d that is used with actual measurement for
the correction of heading angle measurement.
4.2 Filter Structure
The Standard Non Linear Extended Kalman Filter is implemented for sens-
ing complete 3D orientation. It combines the absolute reference information
as the observation quaternion Qm with high bandwidth angular rate of gy-
roscope ZG using quaternion based state model as shown in figure 4.3.
The inner data flow of quaternion based Extendend Kalman filter for ori-
entation estimation is recursively in nature as shown in figure 4.3 by the
feedback arrows. In subsequent iterations it reduces propagation of state
error present in posterior state quaternion by the inherent statistical pred-
ication computations and prior quaternion .Finally this approach reduced
the state estimation error.
4.2.1 Quaternion System State Model
The state space representation of quaternion based continues time non lin-
ear stochastic system can be written as.
q′(t) =1
2U(ωZG(t)) + v(t) (4.9)
Where, q′(t)-Quaternion state vector of dimension 1×4,[
q0 q1 q2 q3
]
.
U - Conversion matrix of dimension 4 × 3.
4.2 Filter Structure 52
State Prediction Covariance
State Prediction Covariance
Residual Covariance
Residual Covariance
Filter Gain
1)
Filter Gain
1)
Filter Gain
Update State Covariance
Update State Covariance
P(k|k)
Q(k)
R(k)
w(k)
z(k)
v(k)
u(k)
q(k)
Measurement Model
)
Measurement Model
)
Quaternion State Model
)
Quaternion State Model
)
Evaluation of Jacobians
Evaluation of Jacobians
q(k)
Figure 4.3: Process Flow of Quaternion Based Extended Kalman Filter,
P (k|k) is state covariance, Q(k) is measurement Covariance, R(k) is Sys-
tem Covariance, w(k) represents Measurement Noise, z(k) represents obser-
vations equivalent to measurement quaternion Qm , v(k) represents addi-
tional System Noise, u(k) is gyro rate input equivalent to ZG and q(k) is the
quaternion state vector to be estimated by the predictive process of extended
Kalman filter, q, z are state and measurement prediction respectively and
v(k + 1) measurement residual.
4.2 Filter Structure 53
ωZG(t)- Gyro Angular rate vector of dimension 1 × 3,[
ω0 ω1 ω2
]
.
v(t)- System noise or gyroscope noise model.
Equation 4.9 is also known as dynamic equation or the planet equation. In
case of orientation measurement, it is called quaternion state space model.
The term system is used for the orientation sensor module included tri axis
accelerometer, gyroscope and magnetometer.
Conversion Matrix which transform the gyroscope angular rate into the
equivalent quaternion state as shown in ??.
U =1
||q||
−q1 −q2 −q3
q0 −q3 −q2
q3 q0 −q1
−q2 q1 q0
, ||q|| =√
qq′ (4.10)
The discretization or integration of 4.9 gives the quaternion based Kalman
state equation as shown in 4.11 and 4.12.
q(k + 1) = f [k, q(k) + u(k)] + v(k)] (4.11)
q(k + 1) = qk +1
2U(T ∗ ωZG(t)) + v(t) (4.12)
4.12 is known as discrete quaternion state equation. It serves two purposes,
first convert the gyro angular rate into 3D orientation by integration and
second incorporate prior state to the posterior state. Where T in 4.12 is
the sampling period at which the data is being acquired by the orientation
sensor module.
4.2 Filter Structure 54
The system noise v(k)in quaternion based model is noise present in gyro-
scope. It can be model as the sequence of zero mean white Gaussian noise
with covariance as indicated 4.39
E[v(k)v′(k)] = Q(k) (4.13)
4.2.2 Measurement Model
The output of the system or the observation quaternion obtained by the
accelerometer and magnetometer can be represent as
z(t) = q(t) + w(t) (4.14)
Where, z(t)- Measurement Quaternion Qm. q(t)- Quaternion state vector.
w(t)- Measurement Noise, Measurement quaternion noise model.
The discretization of 4.14 provides the measurement equation for discrete
extended Kalman filter as 4.15.
z(k + 1) = h[k, q(k + 1), w(k)] (4.15)
z(k) = q(k) + w(k) (4.16)
z(k) in 4.15is the measurement quaternion Qm as stated earlier. The
measurement quaternion Qm is obtained by converting sensor frame ac-
celerometer and magnetometer measurements into Earth Fixed coordinate
frame of reference.
In Theory, the Earth fixed Frame is known as World Frame as shown in
figure 4.4. It represent as the 3D right hand frame attached to the cen-
ter of the Earth Globe in such a way that Z points towards Geomagnetic
4.2 Filter Structure 55
Z (Geographic North pole)
X (East)Y (Prime Meridian)
Magnetic North
Magnetic South
The Earth
Figure 4.4: Orientation in Earth Fixed Frame
north, Y points towards East and X towards prime meridian. It is de-
noted as superscript W. It will act as the reference frame for the complete
orientation of body frame sensor module. The body frame or the sensor
frame is the frame attached to the orientation sensor module as discussed
in experimental setup. It is denoted as superscript s.
The orientation of sensor frame S relative to Earth fixed frame W can be
calculated by fusing gravity based inclination using accelerometer gs and
magnetic field based heading using Magnetometer ms, according to world
frame. These measurements in sensor coordinate frame can be expressed
in Earth fixed coordinate frame with the use of Rotation matrix as shown
in 4.17 and 4.18.
gw = Rws g
s (4.17)
mw = Rwsm
s (4.18)
Where, gw- Gravity vector in Earth Fixed Frame. gs- Gravity vector in
Sensor Frame. mw- Magnetic field vector in Earth Fixed Frame. ms-
4.2 Filter Structure 56
Magnetic field vector in Sensor Frame. Rws - Rotational Matrix, transform
Sensor frame relative to Earth Fixed Frame.
Rws Rotation matrix represents rotation transformation of sensor frame with
respect to Earth Fixed frame. It is represented by the Euler angular matrix,
roll(φ), pitch (θ) and yaw (ψ) as shown in 4.19.
Rws = RZY X =
cos θ cosψ − cosφ sinψ + sinψ sin θ cosψ sinφ sinψ + cosφ sin θ cosψ
cos θ sinψ cosφ cosψ + sinφ sin θ sinψ − sinφ cosψ + cosφ sin θ sinψ
− sin θ sinφ cos θ cosφ cos θ
(4.19)
Equations 4.17 and 4.18 are non linear equation which can be solved
simultaneously to obtain Euler angles. There are two ways for solving
these equation one is using Gauss Newton Method and other is by the
use of Geographical constrain, such as field of gravity vector and magnetic
north in the Earth fixed Coordinate as presented in (Tatsuya Harada
and Sato, 2003).According to the geographical constrain approache ,the
following gravity and magnetic field vector in Earth fixed frame can be
assumed as 4.20 and 4.21.
gw =[
0 0 −1]T
(4.20)
mw =[
a 0 b]T
(4.21)
Where, a and b are the normalized horizontal and vertical component of
Earth magnetic field respectively. Now, the rotation in sensor frame can
also be expressed as 4.22 and 4.23.
gs = Rswg
w (4.22)
4.2 Filter Structure 57
ms = Rswm
w (4.23)
Where, Rsw = Rw
sT , superscript T indicates Transpose.
By applying the gravitation geographical constrain of the Earth Fixed co-
ordinate in 4.22, the gs can be obtained as 4.24
gs =[
sin θ − sinφ cos θ − cosφ cos θ]T
(4.24)
Using equation 4.22, the Euler roll φ, and pitch θ can be written as 4.25
and 4.26 respectively.
roll − φ = Atan2(
−gsysign(cos θ),−gszsign(cos θ))
(4.25)
pitch− θ = Atan2(
gsx,√
(gsy)2 + (gsz)
2)
(4.26)
Since, the gravity based geographical constrain can only give Euler roll
and pitch angles using accelerometer measurements. As stated earlier for
complete 3d orientation the yaw angle is required .This problem has been
resolved by the use of magnetometer measurement.
The rotation matrix Rws shown in eq:RotationMatrixZYX can be further
split in terms of roll-φ, pitch-θ and yaw-ψ rotation matrix as 4.27.
Rws = RZ(ψ)Ry(θ)Rx(φ) (4.27)
Using equation 4.27, the equation 4.18 can be simplified as 4.28.
4.2 Filter Structure 58
mw = RZ(ψ)Ry(θ)Rx(φ)ms (4.28)
Equation 4.28 can be further simplified as 4.29.
RZ(ψ)Tmw = Ry(θ)Rx(φ)ms (4.29)
By applying geographical magnetic field constrains in equation 4.29, the
yaw angle can be derived as 4.30.
yaw − ψ = Atan2(r(2), r(1)) (4.30)
Where, r − vector = Ry(θ)Rx(φ)ms.
This approach is followed to obtain low frequency Euler rotations by the fu-
sion of accelerometer and magnetometer measurements. In order to obtain
the measurement quaternion Qm, [Qm0Qm1Qm2Qm3]T (z(k)) the follow-
ing equation 4.31− 4.32 are used. These equations convert the Euler angle
into the quaternion based vector. It is necessary to use these equations in-
stead of Euler angles vector that exhibits the properties of singularity which
can only be eliminated with the use of equations 4.31− 4.32.
Qm0 = cosφ
2cos
θ
2cos
ψ
2+ sin
φ
2sin
θ
2sin
ψ
2(4.31)
Qm1 = sinφ
2cos
θ
2cos
ψ
2+ cos
φ
2sin
θ
2sin
ψ
2(4.32)
Qm2 = cosφ
2sin
θ
2cos
ψ
2+ sin
φ
2cos
θ
2sin
ψ
2(4.33)
4.2 Filter Structure 59
Qm3 = cosφ
2cos
θ
2sin
ψ
2+ sin
φ
2sin
θ
2cos
ψ
2(4.34)
The Measurement Noise w(k)in Quaternion is represented by the noise
present in Quaternion Measurement vector using accelerometer and mag-
netometer. It can be assumed as zero mean white Gaussian noise with
covariance as 4.40.
E[w(k)w′(k)] = R(k) (4.35)
4.2.3 Update State Covariance
In linear Kalman Filter the update state covariance was independent to the
state estimation. However in non linear Extended Kalman filter the state
and covariance update are simultaneously computed under the Kalman it-
erative process. It is occurred due to the linearization of non Linear Kalman
state equation which introduces the term evaluation of jacobian. n. The
evaluation of jacobian is the part of update covariance computation as
shown in figure 3 kalman flow diagram. It includes the mathematical com-
putation of jacobian of state and measurement equations. Mathematically,
it can be expressed as 4.36 and 4.37.
F (k) =∂q(k)
∂qq=q(k|k) (4.36)
H(k + 1) =∂q(k)
∂q q=q(k+1|k)
(4.37)
By applying the mathematical jacobian on 4.12 and 4.16 the F(k) and H(k)
can be evaluated respectively. It is the essential step in extended kalman fil-
ter for updating state covariance. The intermediate steps of updating state
4.2 Filter Structure 60
covariance as shown in figure 4.3, such as state predication covariance and
residual covariance that incorporate the system Q(k) and measurement
R(k) noise covariance for estimating the filter gain. The weight of the filter
gain is mainly reduced the error in current state estimation. Consequently
the inherent Kalman structure able to filtered additional process and mea-
surement noises.
As can be seen in figure 4.3 the filter gain which reduces state estimation
error depends on the accurate knowledge of system Qk and measurement
Rknoise covariance input. Therefore, these covariances are also known as
error covariance. They utilized to predict the state predication and residual
error covariance. Finally, the filter gain is obtained that uses to estimate
state and covariance update. The updated state covariance that combine
filter gain, residual covariance and prior covariance for the next iteration
can be written as 4.38.
P (k + 1|k + 1) = P (k + 1|k) −W (k + 1)S(K + 1)W ′(k + 1) (4.38)
4.2.4 Error Covariance
As stated earlier section the knowledge of error covariance Qk and Rk
are the important factor to compensate the affect of external noises. In
quaternion based orientation included accelerometer, magnetometer and
gyroscope, they can be modeled as zero mean white Gaussian noise. The
system error covariance Q(k) that describes the process noise w(k) or the
noise present in gyroscope can be experimentally obtained by the measure
of error propagation in gyroscope. It can be represented as the diagonal
matrix shown in 4.39.
4.3 Experiement 61
Qk =
Qk0 0 0 0
0 Qk1 0 0
0 0 Qk2 0
0 0 0 Qk3
(4.39)
In similar way, the noise present in accelerometer and magnetometer mea-
surement as the function of v(k) can be described by the measurement error
covariance R(k). It represent as the 4×4 diagonal matrix as indicated 4.40
whose diagonal elements are experimentally computed by the measure of
error propagation in accelerometer and magnetometer.
Rk =
Rk0 0 0 0
0 Rk1 0 0
0 0 Rk2 0
0 0 0 Rk3
(4.40)
4.3 Experiement
The purpose of the experiment was to investigate the stability and perfor-
mance of developed sensor module using Quaternion Kalman Filter in pres-
ence of environment disturbances and noises. For the experiment in various
condition, the low cost 3D Orientation sensor module was constructed as
indicated in figure 4.5. As can be seen in Figure the 3D orientation sensor
is comprises on 2 layers of PCB that are stacked together in one package
called module. The top layer of PCB (Printed Circuit Board) contains
two HM55B Hitachi Magnetic Compasses that are orthogonally positioned
with the help of two small PCBs. Each small PCB holding HM55B sensor
is mounted orthogonally on the top layer PCB as shown in figure. This
configuration measure the complete 3d Magnetic Field Vector.
4.3 Experiement 62
Figure 4.5: 3D Orientation Sensor Module, Module is depicted in various
views with their corresponding sensor reference frame is indicated by the
x(Red) ,y(Black) and Z( Green) at the bottom corner
In similar way, the Analog Inertial Measurement Unit AD16350 included
tri axis accelerometer and gyroscope is routed on the bottom layer PCB as
square box indicated in figure 5. Both sensors are digital and produces their
outputs in SPI Protocol standard. They are connected with the acquisition
board containing ATMEAG128 low cost controller as stated earlier in last
chapter. The data acquisition controller was operating at the 14.756 MHZ
clock frequency. At this frequency controller provide baud error free se-
rial data stream to the computer. The controller performed two functions.
First, acquired the data by the sensors using conventional I/O ports and
bundle them into the form of packet before transmitting to the host com-
puter. It takes fractions of mille seconds. Second maintain the constant
throughput or the data samples for transmission that is essential in case
4.3 Experiement 63
of real-time discrete Kalman Filter implementation. Finally, the acquired
data packet was being transmitted to the computer at the sample rate of
0.136 sec. A simply formula can also be used to compute the sample rate
as shown in 4.41.
ts = ta + tt (4.41)
Where, ts - Packet sampling time. ta - Sensor data acquisition time. tt -
Data sampling time.
The complete performance measurement experiment is divided into the
following cases.
1. When the orientation sensor is at rest.
2. When the orientation sensor is influenced by an external magnetic
disturbance.
Before starting an experiment, the bias factor and gain multiplier of each
sensor has been computed by the calibration as presented in Appendix A.
In calibration, each sensors including accelerometer, gyroscope and magne-
tometer measurements is observed for 8 -10min in quasi static case.Finally
the observation is then utilize to obtain the element of error covariance for
discreet Kalman filtering.These valuse were not changed during the exper-
iment.
4.4 Results 64
Pitch
θ
Roll
Ф
Yaw
ψ
Orientation Sensor
Figure 4.6: Orientation sensor rotation about Roll φ , Pitch θ and Yaw ψ,
anticlockwise about an each axis is +ve deg and clockwise is -ve deg.
4.4 Results
4.4.1 Case 0: Influence of Environment Noises
In case one the developed orientation sensor module was at rest or static
in position about the period of 30 min. Approximately 13050 total no of
samples has been taken by the experiment as shown in figure4.7. As can be
seen in observed measurement graph still the impact of bias drift is more
dominate in gyro measurement as compared to the accelerometer and mag-
netometer measurements. Therefore the Quaternion based Kalman filter
was required. Finally the 3D orientation result obtained by the Kalman
estimation and without Kalman estimation is indicated in figure4.8.
In figure 4.8, it can be seen that by the use of Kalman estimator the impact
of noise in 3d orientation has been considerably reduced up to 0.3 deg. In
another way the accuracy can also be measured with the use statistical
quantity such as standard deviation that measure the dispersion of set of
values as listed in table 4.1. In comparison, less the standard deviation
more the system is accurate and stable. It proved that the accuracy of 3d
orientation measurement with the use of Kalman filter is much better than
without kalman filter.
4.4 Results 65
0 2000 4000 6000 8000 10000 12000 14000−5
0
5
10
15Tri Axes Acceleration (m/s2)
T Sample
Acc
eler
atio
n m
/sec
2
0 2000 4000 6000 8000 10000 12000 14000−0.04
−0.02
0
0.02
0.04Tri Axes Gyro (rad/s)
T Sample
Ang
ular
vel
ocity
rad
/sec
0 2000 4000 6000 8000 10000 12000 14000−100
−50
0
50Tri Axes Magneto (H)
T Sample
H u
Tes
la
Accel XAccel YAccel Z
Gyro XGyro YGyro Z
Magno XMagno YMagno Z
Figure 4.7: 30min Tri Axis Acceleration, Gryroscope and Magnetometer
observation.
Further this can also be confirmed with the help of statistical histogram
of Euler angle with and without kalman filter as shown in figure4.9. In
Figure4.9 the red line is indicated histogram fit Gaussian distribution of
the sets of Euler angles. It shows that if the set of data or measurements is
more converge to the mean value and width of Gaussian which is defined
by the twice of standard deviation is narrow than the system is more sta-
ble and accurate. It can be clearly seen that by the use of Kalman Filter
S.No Standard Deviation σ Without Kalman Filter With Kalman Filter
0 Roll φ 0.1595 0.0973
1 Pitch θ 0.1963 0.1462
2 Yaw ψ 2.6744 1.1482
Table 4.1: Standard deviation of Euler angles with and without Kalman
estimation.
4.4 Results 66
0 2000 4000 6000 8000 10000 12000 14000−2
−1
0
1Roll Angle
T Sample time
Deg
Without Kalman FilterWith Kalman Filter
0 2000 4000 6000 8000 10000 12000 14000−1
0
1
2Pitch Angle
T Sample time
Deg
Without Kalman FilterWith Kalman Filter
0 2000 4000 6000 8000 10000 12000 1400080
90
100
110Yaw Angle
T Sample time
Deg
Without Kalman FilterWith Kalman Filter
Figure 4.8: 3D Euler orientation measurement at rest.
the measurement has been more converge to their mean as compared to
without Kalman filter. Moreover, the outliers are also less compared to the
histogram without Kalman filtering in which the population of measure-
ment is not completely bounded within fitted Gaussian distribution curve.
It can also be noticed that the impact of white noise in orientation sensor
module is Gaussian in nature as stated earlier.
4.4.2 Case 1: Influence of the external magnetic dis-
turbance
In this case the performance of Kalman Filter is observed under the affect
of additional magnetic disturbance by the use of Ferromagnetic electric
iron. During the experiment the orientation sensor was placed on the table
4.4 Results 67
−1.5 −1 −0.5 0 0.5 10
1000
2000Histogram of Roll Angle Without Kalman Estimator
−0.6 −0.4 −0.2 0 0.2 0.40
200
400
600Hist of Roll Angle With Kalman Estimator
−1 −0.5 0 0.5 1 1.50
500
1000
1500Histogram of Pitch Angle Without Kalman Estimator
−1 −0.5 0 0.5 10
200
400
600Hist of Pitch Angle With Kalman Estimator
80 85 90 95 100 1050
200
400
600Histogram of Yaw Angle Without Kalman Estimator
88 90 92 94 96 980
200
400Hist of Yaw Angle With Kalman Estimator
Figure 4.9: Histogram comparison of 3D orientation with and without
Kalman estimator.
and the electric iron was moved near to the sensor from the x direction
to the z direction and after few seconds it has been moved away by the
sensor directly from the z direction. The duration of magnetic disturbance
is indicated by two arrow ended horizontal line shown in figure 4.10.It can
be seen that the two X and Z component of magnetic vector has been more
affected by the magnetic disturbance instead of Y component. Using same
measurement the kalman filter has been applied which finally reduced the
impact of disturbances comparable to the measurement without Kalman
filter as shown in figure 4.11. By the statistical view still the standard
deviation in case of kalman filtered measurement is less than the standard
deviation without kalman filter as indicated in table 4.2.
In figure 4.12 the statistical boxplot of yaw measurement with and without
kalman is shown. It clearly shows that the outliers in yaw measurement
4.4 Results 68
0 50 100 150 200 250−90
−80
−70
−60
−50
−40
−30
−20
−10
0Tri Axes Magneto (H)
T Sample
Nor
mal
ized
H
Magno XMagno YMagno Z
Magnetic Disturbance
Figure 4.10: Tri Axis Magnetic Vector disturbance due to ferromagnetic
electric iron.
0 50 100 150 200 250105
110
115
120
125
130
135
140Yaw Angle
T Sample time
Deg
Without Kalman FilterWith Kalman Filter
Additional Magnetic Disturbance
Figure 4.11: Yaw angle measurement with and without Kalman filter under
magnetic disturbance.
4.4 Results 69
without Kalman estimation are much higher than the yaw measurement
with Kalman estimator. Further the median value in case of Kalman fil-
tered data is also less than the median value without Kalman estimation
which is slightly shifted toward the upper quartile. Finally, it proves that
the influence of magnetic disturbance can affect the accuracy of the angle
measurement with and without Kalman Filter but still the use of Kalman
filter is providing smooth and stable angle measurement.
S.No Standard Deviation σ Without Kalman Filter With Kalman Filter
0 Yaw ψ 9.3612 6.8292
Table 4.2: Standard Deviation of Yaw Angle with and without Kalman
Estimation.
Yaw Without Kalman Yaw With Kalman0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Val
ues
Column Number
Euler Yaw Box Plot
Figure 4.12: BoxPlot of Yaw measurement with and without Kalman.
4.5 Discussion 70
4.5 Discussion
The experimental results show that the use of kalman estimation is neces-
sary in inertial and magnetic measurement for complete and accurate 3d
orientation measurement. The various aspects of noise and magnetic dis-
turbance has been analyzed by the experiment which proves that the 3d
orientation measurement using Kalman estimation are accurate up to 0.2
deg along each orientation axis. It has also been noticed by the experi-
ment. The accuracy of 3d orientation measurement is depends on several
factor such as motion speed, sampling rate, dynamic magnetic disturbance
and environment temperature. However, the increase in motion speed and
amount of magnetic disturbance can more affect the accuracy comparable
to other aspects. Despite that the proposed estimation algorithm is also
tested in real-time that will be required for capturing human arm motion
sequences. It will be discussed further in next sections.
In case when the sensor is placed near to the ferromagnetic material that
can affect the accuracy of the 3D orientation measurement the proper cali-
bration would be necessary. It enhances the performance of the estimation
filter. It has also been observed that the increase in the ferromagnetic ma-
terial and magnetic disturbance can significantly impact on the yaw angle
measurement and also decrease the efficiency of estimation filter. Further
when the orientation module is placed in car for navigational application
then the amount of unnecessary acceleration must be take into account to
enhance the accuracy of 3d orientation measurement because the increase
in linear acceleration can result of wrong inclination measurement. By con-
sidering this effect of acceleration the low pass pre filter is implemented in
the algorithm.
4.5 Discussion 71
The proposed estimation algorithm can be applied to any combination of
tri axis accelerometer, gyroscope and magnetometer for complete 3D orien-
tation measurement. In practical, the specification of the sensors should be
known such as noise and bias drift. By the advancement in MEMS technol-
ogy it could be possible to achieve less drift Gyros in future. It means that
the more accuracy could be achieved by the same approach. Each sensor
is temperature dependent; therefore the affect of temperature drift can be
modeled further to improve the accuracy. Besides the accuracy can also
be enhance by the implementation of adaptive estimation filter. Although
it would require more computation but this approach can significantly re-
duced the effect of external noises and disturbance.
Finally the proposed filter is more efficient to obtain the 3d orientation for
the robotic application which mainly requires the accuracy of 1 deg. Its ca-
pability can be further enhanced by the fusion of other angle measurement
method.
Chapter 5
Kinematic Analysis & Modeling
of WorkPartner Manipulator
The recent development and advancement in the field of robotics are gener-
alizing the concept of robots in human world. The robot is "The intelligent
mechanical structure that supposes to function autonomously" ??. The
intelligent means the robot does not do its task in a repetitive way as the
machine currently being used in the Automation industry; instead it is
the machine that utilizes its computer (brain) and mechanical structure to
solve real world problems without recourse of human operator. In practical,
the concept of such robots is far from the present technology require more
advancement. Thus, the concept of semi autonomous robot is still popular.
Based on these facts the field service robot, called WorkPartner ?? is de-
veloped. It is the semi autonomous robot that can function autonomously
and non-autonomously. It depends on the nature of the task. It is designed
to help human in their routinely tasks in urban environment. The Mechan-
ical Structural of WorkPartner Robot is hybrid as shown in figure 5.1 . It
73
allows the robot to be more efficient for rigorous outdoor applications. As
can be seen in figure 5.1, the complete WP can be viewed into two parts,
front part called "body" which is human like and back part called "panel"
which is car like. The panel of the WP robot included four active wheels
(walking and rolling) and power sub-system (combustion engine and batter-
ies) increase the locomotion on uneven terrain and fulfill power requirement
during the task. While the body part included Perception system and two
human arms like manipulators, enable the robot to sense dynamic work
environment and utilize manipulator to accomplish various tasks such as
cutting trees, grabbing thing etc.
According to the context of the thesis only the WorkPartner manipulator
is involved. Thus, this chapter is focused on the kinematic analysis of WP
Manipulator followed by its mechanical specification. Finally, the motion
has been analyzed and discussed using developed 3d kinematic simulator
of the WorkPartner Robot.
Perception System
Manipulator
Power System
Active Wheel Joint
Figure 5.1: 3D CAD model of WorkPartner Robot
5.1 Specification of WorkPartner Manipulator 74
5.1 Specification of WorkPartner Manipula-
tor
Any Robot which has human like manipulator comprising motion in 7DOF,
can do the task that human arm can do. Considering this fact the human-
like manipulator of the WorkPartner Robot has been designed. However
its motion is limited in 5DOF. Despite that it is functioned efficiently in
the work environment. The motion of WorkPartner manipulator in 5DOF
is achieved by the combination of 3 revolute joint. As can be seen in figure
5.2 the 5Dof WorkPartner manipulator includes 2 DOF shoulder joint, 1
DOF Elbow and 2 DOF Wrist. . While the end effector is not completely
as the human hand comprises five fingers but it can be gripper or cutter
etc. Its choice is mainly depends up on the application.
Shoulder
Inclination (pitch)Shoulder
Tilt (Yaw)
Elbow
Inclination (pitch)
Wrist
Inclination (pitch)
Wrist
Rotation (Roll)
2DOF
1DOF
2DOF
Figure 5.2: Joint Specification of WorkPartner Manipulator
Each 1DOF joint motion of the manipulator is controlled by DC servo
motor with tailor made planetary gears ??.It enables the manipulator to
attain position in 5DOF work envelope with the accuracy of 1deg. The
brake system is also installed in joint which can be used to manually repo-
5.2 Kinematic Modeling of WP Manipulator 75
sition the manipulator during uncontrolled state.
5.2 Kinematic Modeling of WP Manipulator
Denavit Hartenberg notation is simplest way of modeling mechanical ma-
nipulator for kinematic analysis. It is mainly use to define the mechanics
of rigid body or chain of rigid body in terms of 1 joint variable and 3 fixed
link parameters. For the revolute or rotary joint the θ is the joint variable
that defines movement of attached segment in work space with additional
three fixed link parameter known link length ai, link twist αi and link offset
di. In case of prismatic joint the link offset di is the joint variable and other
three are fixed. Further detail can be found in ??. As the single sided WP
manipulator is only comprised on three revolute joints. Therefore, it can
be modeled using DH notation as shown in table 5.1.
S.NO Link(i) αi ai di θi Angle range
1 Shoulder(Tilt) L1 00 1 0 θ1 −450 − 450
2 Shoulder(Inc) L2 −900 0 0 θ2 −900 − 900
3 Elbow(Inc) L3 00 4 0 θ3 00 − 1400
4 Wrist(Inc) L4 00 4 0 θ4 −900 − 900
5 Wrist(Rot) L5 −900 0 0 θ5 −900 − 900
6 End Effector L6 00 1 0 θ6 −
Table 5.1: Denavit Hartenberg representation of WP Manipulator
The Table 5.1 has been used to generate the Matlab compatible simulation
model using Robotic Toolbox as shown in figure 5.3. It has been used to
analyze the kinematic motion of WP in Workspace. Although, it is an
ideal kinematic model of WP Manipulator that cover complete workspace
as the sphere whose radius is equivalent to the length of the Manipulator.
5.2 Kinematic Modeling of WP Manipulator 76
In Practical, the joint motion of the WP Manipulator is not completely
cover 360 deg as indicated in angle range column of DH table. Therefore,
the workenvelop of the WorkParnter can be anaylzed using this simulation
model by limiting the motion of each joint according to its angle range
aspects.
−4−3
−2−1
01
2
−4
−2
0
2−4
−3
−2
−1
0
1
2
XY
Z
XY
Z
Figure 5.3: DH Kinematic Model of WP Manipulator
5.2.1 WorkEnvelop
The Work envelope of any manipulator defines the area in which it can
be moved easily. Further, it shows the reach ability of the manipulator.
In case of 5 DOF WorkPartner Manipulator, where each revolute joint
motion is not completely covered 360 deg. Thus, the Work enevelope has
been analyzed using DH kinematic simulation model in matlab. Based
on that the work envelop is showed in 3d kinematic model of the WP in
figure below. This 3d model is implemented for real-time emulation and
translation of human motion into the robot manipulator equivalent motion
5.2 Kinematic Modeling of WP Manipulator 77
as explained further in next chapter.
S_inc max 90o
S_inc min -90o
S_inc 0o
E_inc min 0o
E_inc max 140o
w_
inc
0o
w_
inc
ma
x 9
0o
S_tilt max 45o
S_tilt min -45o
S_tilt 0o
Figure 5.4: WorkEnvelop of Single WP Manipulator,a)Left Side view, b)
Top viewShoulder motion range is -45 deg-45 deg in tilt and -90 deg-90
deg in inclination, Elbow motion is in 0 deg-140 deg inclination followed
by shoulder motion, Wrist motion is -90 deg-90 deg in both inclination and
rotation followed by the shoulder and elbow motion.
As can be seen in figure 5.4 the 2DOF shoulder motion is indicated as
the purple quarter sphere, whose radius is equivalent to the length of the
manipulator. It signifies that the complete motion of WP robotic arm can
be moved within it. While the light green pie in left side of figure indicates
that when the shoulder joint will have reached its maximum inclination
then the Elbow motion follows the green pie. In similar way, when the
elbow motion has reached its maximum inclination then the wrist motion
follow the small reddish pie. Further, the span of each joint motion is also
indicated as line with values.
References
Aarne Halme, Ilkka Leppänen, J.S.S.Y. and Kettunen, I.
(2003). WorkPartner: Interactive Human-Like Service Robot for Out-
door Applications. In The International Journal of Robotics Research.
Helsinki University of Technology, Automation Technology Labora-
tory.
Angelo M, S. (2006). Quaternion-Based Extended Kalman Filter
for Determining Orientation by inertial and Magnetic Sensing. In
Transactions on Biomedical Engineering. IEEE.
Ashutosh Saxena, Gaurav Gupta, V.G. and ebastien
Ourselin (2004). In Use Parameter Estimation of Inertial Sensors
by Detecting Multilevel Quasi-static States. Department of Electrical
Engineering, Stanford University, USA.
ASIMO, H. (2008). BibTeX — Honda, The ASIMO Humanaiod
Robot. [Online; accessed 5th-June-2008].
URL: http: // asimo. honda. com
Daniel Roetenberg, C.T.B. and Veltink, P.H. (2007a). Es-
timating body segment orientation by applying inertial and magnetic
sensing near ferromagnetic materials. In IEEE TRANSACTIONS OF
NEURAL SYSTEMS AND REHABILITATION ENGINEERING.
REFERENCES 79
Daniel Roetenberg, P.J.S. and Veltink, P.H. (2007b). Ambu-
latory Position and Orientation Tracking Fusing Magnetic and Inertial
Sensing. In TRANSACTIONS ON BIOMEDICAL ENGINEERING.
IEEE.
David H. Titterton, J.L.W. (2004). Inertial Navigation. In Strap-
down Inertial Navigation Technology - 2nd Edition. The institute of
Electrical Engineers.
Demox Gebre-Egziabher, Gabriel H. Elkaim, J.P. and
W.Parkinson, B. (1998). A Gyro-Free Quaternion-Based Attitude
Determination System Sitable for Implementation using Low Cose
Sensors. Department of Aeronautics and Astronautics, Stanford Uni-
versity.
Demoz Gebre-Egziabher, R.C.H. and Powell, J.D. (1998). A
Low-Cost GPSfinertial Attitude Heading Reference System (AHRS)
for General Aviation Applications. Department of Aeronautics and
Astronautics, Stanford University.
DEVICES, A. (2008). BibTeX — ANALOG, Datasheet of
ADIS16530. [Online; accessed 15th-Feburary-2008].
URL: http: // www. analog. com/ UploadedFiles/ Data\ _Sheets/
ADIS16350\ _16355. pdf
Egziabher, Demoz Gebre; Elkaim, G.H.P.J.D.P.B.W. (2008).
A Gyro-Free Quaternion-Based Attitude Determination System Suit-
able for Implementation Using Low Cost Sensors. Department of Aero-
nautics and Astronautics, Stanford University.
Eric Foxlin, Michael Harrington, Y.A. (2004). Miniature 6-
DOF inertial system for tracking HMDs. In Helmet and Head-Mounted
Displays III, pages 1–15. InterSense, Inc, 73 Second Ave., Burlington,
MA 01803.
REFERENCES 80
Eric R. Bach, X.Y. and McGhee, R.B. (2003). Sourceless Track-
ing of Human Posture Using Small Inertia & Magnetic Sensors. In
Computational Intelligence in Robotics and Automation. The MOVES
Institute Naval Postgraduate School, Monterey CA.
Eric R. Bachmann, Robert B, M.X.Y.M.J. (2001). Inertial and
Magnetic Posture Tracking for Inserting Humans Into Networked Vir-
tual Environments. MOVES Institute, Monterey CA.
Everett, H. (1995). Sensors For Mobile Robots Theory & Applica-
tion. Massachusetts : A K Peters, Ltd.
Gypsy6, A. (2008). BibTeX — Inition, Gypsy6 Torso. [Online;
accessed 5th-June-2008].
URL: http: // www. inition. com/ inition/ product. php? URL\
_= product_ mocaptrack_ animazoo_ gypsy&SubCatID\ _= 0
James, D. (2007). Accelerometer Design & Applications. Analog
Devices.
URL: http://www.analog.com/Analog_Root/static/library/techArticles/mems/sensor971
James F, O.Brien Robert, E.B.J.G.J.B.J. and Hodgins, K.
(2000). Automatic Joint Parameter Estimation from Magnetic Motion
Capture Data.
John Geen, D.K. (2008). New iMEMS R© Angular-Rate-Sensing Gy-
roscope. ADI Micromachined Products Division.
Johnston, O. Thomas, F. (1981). The Illusion of Life: Disney
Animation, Disney Editions,. Disney.
Karim Abdel-Malek, Jingzhou Yang, W.Y. (2003). Human
Performance Measures: Mathematics.
REFERENCES 81
Kraft, E. (2003). A Quaternion-based Unscented Kalman Filter
for Orientation Tracking. ISF, Physikalisches Institute, University of
Bonn.
L. Herda, P. Fua, R.P.R.B. and Thalmann, D. (2000). Skeleton-
Based Motion Capture for Robust Reconstruction of Human Motion.
Computer Graphics Lab (LIG) EPFL CH-1015 Lausanne, Switzerland.
leonardo-da-vinci biography (2008). BibTeX — LDVB, Vitru-
vian man and Da Vinci surgical anatomy drawings. [Online; accessed
5th-June-2008].
URL: http: // www. leonardo-da-vinci-biography. com/
vitruvian-man-anatomy. html
Luinge, H.J. and Veltink, P.H. (2004). Inclination Measurement
of Human Movement Using a 3-D Accelerometer With Autocalibration.
In TRANSACTIONS ON NEURAL SYSTEMS AND REHABILITA-
TION ENGINEERING. IEEE.
Magnet.fsu (2008). BibTeX — magnet.fsu, Interactive Hall Effect.
[Online; accessed 15th-Feburary-2008].
URL: http: // www. magnet. fsu. edu/ education/ tutorials/
java/ halleffect/ index. html
Marcelo C. Algrain, J.Q. (1993). Accelerometer Based Line-
of-Sight Stabilization Approach for Pointing and Tracking Systems.
In Conference on Control Applications. InterSense, Inc, University of
Nebraska-Lincoln.
Marjan A. Admiraal, M.J.K. and Gielen, S.C. (2004). Modeling
Kinematics and Dynamics of Human Arm Movements. Motor Control,
8:312–338.
Marmion, M. (2006). Airborne attitude estimation using a Kalman
REFERENCES 82
filter. The University Centre of Svalbard, University of Nebraska-
Lincoln.
Martin, R.B. (1999). A Genealogy of Biomechanics. Presidential
Lecture presented at the 23rd Annual Conference of the American
Society of Biomechanics.
URL: http://www.asbweb.org/html/history_mission/genealogy/genealogy.htm
Miller, N., J.O.K.M.M.M. (2004). Motion capture from inertial
sensing for untethered humanoid teleoperation. Humanoid Robots,
IEEE/RAS International Conference, 2:547– 565.
Moreno, R.A. (2007). Cognitive Robotics.
nationalatlas (2008). BibTeX — nationalatlas.gov, Geomagnetism:
The Magnetic Field of the Earth. [Online; accessed 15th-Feburary-
2008].
URL: http: // nationalatlas. gov/ articles/ geology/ a\
_geomag. html
Peter H. Veltink, Hans B.J. Bussmann,
W.d.V.W.L.J.M.R.C.V.L. (1996). Detection of Static and Dynamic
Activities Using Uniaxial Accelerometers. In TRANSACTIONS ON
REHABILITATION ENGINEERING, volume 4. IEEE.
REBELLO, K.J. (2004). Applications of MEMS in Surgery. In Pro-
ceedings of the IEEE, volume 92. IEEE.
Rong Zhu, Z.Z. (2004). A Real-Time Articulated Human Motion
Tracking Using Tri-Axis Inertial/Magnetic Sensors Package. IEEE
TRANSACTIONS ON NEURAL SYSTEMS AND REHABILITA-
TION ENGINEERING, 12:295–301.
Satoshi KURATA, Masaaki MAKZKAWA, H.K.A.T.R.T.
(1999). JOINT MOTION MONITORING BY ACCELEROMETERS
REFERENCES 83
SET AT BOTH NEAR SIDES AROUND THE JOINT. In Proceedings
of the 20th Annual Znternational Conference of the ZEEE Engineering
in Medicine and Biology Society.
Schneck, D.J. (2000). The Biomedical Engineering Handbook. vol-
ume 1. CRC Press & IEEE Press.
Sensors, F.B. (2008). BibTeX — Images SI Inc, Sensors Manufac-
turer. [Online; accessed 15th-Feburary-2008].
URL: http: // www. imagesco. com/ sensors/ flex-sensor. html
Stefan Hübschmann, M.S. (1996). Magnetoresistive Sensors Prin-
ciples of Operation and Applications. ZETEX.
URL: http://www.zetex.com/3.0/appnotes/apps/an20.pdf
Stilson, T. (1996). Accelerometer (Analog Devices ADXL50).
URL: http://www.cs.princeton.edu/courses/archive/fall07/cos436/sensors/node9.html
Suomela, J. Halme, A. (2004). Human robot interaction - case
WorkPartner. Intelligent Robots and Systems, 2004. (IROS 2004).
Proceedings, 4:3327– 3332.
Tatsuya Harada, Hiroto Uchnio, T.M. and Sato, T. (2003).
Portable Orientation Estimation Device Based on Accelerometers,
Magnetometers and Gyroscope Sensors for Sensor Network. IEEE,
University of Tokoyo.
Tatsuya Harada, Hiroto Uchnio, T.M. and Sato, T. (2007).
Development of Wireless Networked Tiny Orientation Device for
Wearable Motion Capture and Measurement of Walking Around, Walk-
ing Up and Down, and Jumping Tasks. IEEE, University of Tokoyo.
TKK (2008). BibTeX — TKK, The Automation and System Tech-
nology Dept. [Online; accessed 5th-June-2008].
REFERENCES 84
URL: http: // automation. tkk. fi/ WorkPartner:
WorkPartnerRobot. jpg
Toulmin, S. (2001). History of the Study of Locomotion.
URL: http://www.univie.ac.at/cga/history/renaissance.html
W. Maurel, D. Thalmann, P.H.P.B.P.G.P.K.N.M.T. (2004). A
Biomechanical Musculoskeletal Model of Human Upper Limb for Dy-
namic Simulation. Computer Graphics Laboratory, Swiss Federal In-
stitute of Technolog, EPFL, CH-1015 Lausanne, Switzerland.
Wes Trager, G.S.O. (1999). A Practical Approach to Motion
Capture: Acclaim’s optical motion capture system. [Online; accessed
15th-Feburary-2008].
URL: ’http: // www. siggraph. org/ education/ materials/
HyperGraph/ animation/ character\ %_ animation/ motion\
_capture/ motion\ _optical. htm
Xiaoping Yun, Mariano Lizarraga, E.R.B. and B.McGhee,
R. (2003). An Improved Quaternion-Based Kalman Filter for Real-
Time Tracking of Rigid Body Orientation. In Conference on Intelligent
Robots and Systems. IEEE, Naval Postgraduate School, Monterey,CA
93943.
Xsens (2008). BibTeX — Xsens, Inertial & Magnetic Based Motion
Sensor. [Online; accessed 5th-June-2008].
URL: http: // www. moven. com/ en/ home_ moven. php
Yasuyoshi YOKOKOHJI, Yuki KITAOKA, T.Y. (2002). Motion
Capture from Demonstrator Viewpoint and Its Application to Robot
Teaching. In Proceedings of the 2002 IEEE lntemational Conference
on Robotics 8 Automation.
Yu, T. (2007). Motion Capture (MoCAP) and Motion Data Related
Technologies. UNC Department of Computer Science.
Appendix A
Calibration of Orientation Sensor
Module
The calibration is the process of eliminating or reducing the effect of bias
offset in sensors measurements over the range of all continuous values. In
case of 3d orientation sensor module included tri axis accelerometer, gy-
roscope and magnetometer, it is required for three purposes. First is to
reduce the amount of unnecessary fluctuation (Bias offset) from sensors
measurement. Second is to measure the amount of white noise present in
each sensor as the element of noise covariance matrices for Kalman estima-
tor. Third is to convert the raw measurement in to the corresponding SI
unit standards such as m/s2 for acceleration,rad/sec for gyro measurement
and utesla for magnetometer.
The two Orientation Sensors Modules have been used for the calibration.
One is developed by Xsens technologies and other is self made low cost as
shown in figure A.1. The Xsens orientation sensor module is comprised
on embedded controller and tri axis accelerometer, gyroscope and magne-
A Calibration of Orientation Sensor Module 86
tometer in small package. It directly provides serial raw data stream (not
calibrated) of each sensor in the form of packet on the serial port. Its
sampling rate and the serial baud can be adjusted using sensor configura-
tion command and testing software. While the self developed orientation
module is built by the low cost IMU ADIS16350 and two orthogonally
positioned HM55B Magnetic compass. It requires an additional data ac-
quisition hardware that obtains the raw data stream by the sensors and
bundle them into the form of packet before serial transmission. Both ori-
entation module are using tri axis inertial (accelerometer & gyroscope) and
Magnetometer which are subjected of various inherent and environmental
noise.
Figure A.1: Orientation sensor Modules,Left one module is developed by
Xsens Technologies, Right one is Self developed using ADIS16350 (Analog
Devices) and HM55B (Hitachi).
As observed by the experiment, the fluctuation in MEMS sensor (Ac-
celerometer and Gyroscope) is caused by the change in operating temper-
ature, misalignment of micro electro mechanical mass (drift) and environ-
ment noises. Thus, these sensors must be calibrated before using in actual
system. The following experiments are performed to obtain the amount of
bias offset, which must be reduced or mathematically subtracted later from
the real measurements.
A Calibration of Orientation Sensor Module 87
A.1 Experiment 1: Determination of Qk and
Rk
In this experiment self developed orientation sensor are tested in static case.
That means sensor is not in motion and placed on the work table. Thus,
the measured data stream should be ideally constant. This experiment is
carried out for the period of 30 min because statistically it is enough to
analysis the properties of all sensors. The total no of 13052 data samples
has been recorded by the self made orientation sensor module at the sample
rate of 0.137909 sec, which is equivalent to approximately 7 samples/sec
.By the use of this experiment the influence of environment noise and drift
is computed as the element of error covariance Rk and Qk which can be
used in Quaternion based Extended Kalman filter to filter the impact of
environmental noises.
The experiment was conducted on Date: 15 June 2008 and Time: 5:00
- 5:30 at average vector temperature [9.2194 (X), 7.0413(Y), 6.8282(Z)]
Celsius. The data was being acquired at the rate of 7 sample/ sec. The
observed measurement during 30 min of experiment is shown in figure A.2.
As the covariance is the statistical measure. Therefore, using 30min ob-
servation the histogram of each sensor is plotted as shown in figure 2. It
clearly confirms that the noise is in Gaussian in nature whose mean is at
the peak of Gaussian fitted curve and variance defines the spread of mea-
surement about mean. The variance is actually measure to analyze the
accuracy of each sensor. If any sensor whose variance is near to zero or
in other word the spread of measurement as indicated in histogram is less,
then the sensor is good and accurate comparable to the one whose variance
is greater. Mathematically the mean and the variance can be expressed as
A.1, A.2 respectively.
A Calibration of Orientation Sensor Module 88
0 2000 4000 6000 8000 10000 12000 14000−10
0
10
20Tri Axes Acceleration (g)
T Sample
Acc
eler
atio
n m
/sec
2
Accel XAccel YAccel Z
0 2000 4000 6000 8000 10000 12000 14000−0.05
0
0.05Tri Axes Gyro (rad/s)
T SampleAng
ular
vel
ocity
rad
/sec
Gyro XGyro YGyro Z
0 2000 4000 6000 8000 10000 12000 14000−1
−0.5
0
0.5Tri Axes Magneto (uT)
T Sample
Nor
mal
ized
uT
esla
Magno XMagno YMagno Z
Figure A.2: Tri Axis Accelerometer,Gyroscope and Magnetometer mea-
surement
x = E[x] =
∑N
n=1xnN
(A.1)
Where, x = E[x] , Mean value of x.
xn, set of x measurement data.
N , Total no of samples in x.
varx = E[x− x] =∑N
n=1(xn − x) (A.2)
Using equation A.1, A.2 the mean and variance of each sensor is computed
as shown in table 1:
As observed that each sensor values is not constant as it should be, There-
fore the mean value is used as the bias offset which reduce the impact of bias
A Calibration of Orientation Sensor Module 89
−1 −0.5 00
1000
2000
3000Histogram of Acceleration X
Acceleration m/sec2
N S
ampl
es
−1 −0.5 0 0.50
1000
2000
3000Histogram of Acceleration Y
Acceleration m/sec2
N S
ampl
es
9 9.5 10 10.50
1000
2000
3000Histogram of Acceleration Z
Acceleration m/sec2
N S
ampl
es
−0.05 0 0.050
500
1000Histogram of Gyroscope X
Angular rate rad/sec
N S
ampl
es
−0.05 0 0.050
500
1000Histogram of Gyroscope Y
Angular rate rad/sec
N S
ampl
es
−0.05 0 0.050
500
1000Histogram of Gyroscope Z
Angular rate rad/sec
N S
ampl
es
−0.1 0 0.10
2000
4000
6000Histogram of Magnetometer X
Magnetic Field uT
N S
ampl
es
−0.4 −0.3 −0.20
2000
4000Histogram of Magnetometer Y
Magnetic Field uT
N S
ampl
es
−1 −0.95 −0.90
1000
2000Histogram of Magnetometer Z
Magnetic Field uT
N S
ampl
es
Figure A.3: Histogram fit of Accelerometer,Gyroscope and Magnetometer.
Measurement Type Accel X Accel Y Accel Z Gyro X Gyro Y Gyro Z Magno X Magno Y Magno Z
Mean -0.2372 -0.1337 9.7872 -0.0002 -0.0068 0.0059 -0.0133 -0.2906 -0.9566
Variance 0.0025 0.0021 0.0025 0.0001 0.0001 0.0001 0.0002 0.0001 0.0000
Table A.1: Statistical Observation (Mean and Variance)
A Calibration of Orientation Sensor Module 90
drift present in each sensor. As explained in Chapter 5, the accelerometer
and magnetometer are being used to obtain the measurement quaternion.
Using the same sets of equations the measurement quaternion is computed
by the fusion of 30min accelerometer and magnetometer measurement. The
result is plotted as the histogram of quaternion vector components shown
in figure A.4 .
0.4 0.6 0.80
100
200
300
400
500
600Histogram of q1
−0.04 −0.02 00
200
400
600
800
1000
1200Histogram of q2
−0.02 0 0.020
100
200
300
400
500
600
700
800Histogram of q3
0.7 0.8 0.9 10
100
200
300
400
500
600Histogram of q4
Figure A.4: Histogram fit of Quaternion Measurement Vector Components
Using the Variance equation the variance in quaternion measurement is
estimated as shown in table. These variances will be used as the element
of measurement covariance RK as indicated in equation.
Measurement Type Quat q0 Quat q1 Quat q2 Quat q3
Variance 2.8654e-004 7.1625e-006 4.9397e-006 2.2529e-004
Table A.2: Statistical Observation (Mean and Variance)
A Calibration of Orientation Sensor Module 91
Rk =
2.8654e− 004 0 0 0
0 7.1625e− 006 0 0
0 0 4.9397e− 006 0
0 0 0 2.2529e− 004
(A.3)
It is assumed that this matrix will remains constant throughout the entire
test using Quaternion based Kalman estimation.
In similar way the element of system noise covariance Qk using gyros mea-
surement is computed. First the mean of gyro is used as the bias factor to
reduce the effect of bias offset from gyros measurement, which is also known
as calibrated gyros measurement. Second by the use of discrete integration
algorithm as indicated in figure A.5, resulting quaternion observation is
measured.
Gyroscope
Sensor
Bias Correction
Gs
Bs
Gc
T
(Sampling Time)
Discrete Integration Quaternion Vector
Output
T
Figure A.5: Quaternion Observation using Discrete Integration Algorithm
Finally the result is statistical analyze as the histogram fit of Quaternion
vector component shown figure A.6. Using variance equation the spread
of quaternion measurement is estimated as listed in table. These variances
will be used as the element of System Covariance Qk.It can be noticed that
the element of system covariance is small. Thus, the Quaternion Kalman
Filter requires less time to estimate an accurate 3d orientation.
A Calibration of Orientation Sensor Module 92
0.8 1 1.20
200
400
600
800
1000
1200Histogram of q1
−0.5 0 0.50
50
100
150
200
250
300
350
400
450Histogram of q2
−0.2 0 0.20
100
200
300
400
500
600
700Histogram of q3
−0.5 0 0.50
50
100
150
200
250
300
350
400
450
500Histogram of q4
Figure A.6: Histogram fit of Quaternion Vector Components using Gyro
Angular rate.
Measurement Type Quat q0 Quat q1 Quat q2 Quat q3
Variance 5.0878e-004 0.0084 0.0015 0.0026
Table A.3: Statistical Observation (Mean and Variance)
A Calibration of Orientation Sensor Module 93
Rk =
5.0878e− 004 0 0 0
0 0.0084 0 0
0 0 0.0015 0
0 0 0 0.0026
(A.4)
Note: All the measurement results are taken at the average vector temper-
ature [9.2194 (X) , 7.0413(Y), 6.8282(Z)] Celsius. The variation in tem-
perature during an experiment is also observed in each axis as shown in
figure A.7.
0 2000 4000 6000 8000 10000 12000 140003
4
5
6
7
8
9
10
11Tri Axes Temperature (C)
T Sample
C
Temperature XTemperature YTemperature ZMean Temperature XMean Temperature YMean Temperature Z
Figure A.7: Tri Axis Temperature Observation (30min)
Appendix B
Name of the 2nd appendix
This is the second appendix.