VESTIBULAR DYNAMIC INCLINOMETERAND MEASUREMENT OF INCLINATION PARAMETERS

By

VISHESH VIKAS

A DISSERTATION PRESENTED TO THE GRADUATE SCHOOLOF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT

OF THE REQUIREMENTS FOR THE DEGREE OFDOCTOR OF PHILOSOPHY

UNIVERSITY OF FLORIDA

2011

c© 2011 Vishesh Vikas

2

v�t� �X mhAкAy s� y кoEV smþB।

EnEv Ím к� z m� d�v sv кAy� q� sv dA ॥

Design is a funny word. Some people think design means how it looks. But of course, ifyou dig deeper, it’s really how it works.

- Steve Jobs

3

ACKNOWLEDGMENTS

I would like to express profound gratitude to my advisor, Prof. Carl Crane, for his

invaluable support, encouragement, supervision and useful suggestions throughout this

research work. His moral support and continuous guidance enabled me to complete my

work successfully. I would like to thank Prof. John Schueller, Dr. Warren Dixon, Prof.

Paul Gader and Prof. William Hager for serving on my committee.

I am especially indebted to my parents, Dr. Om Vikas and Mrs. Pramod Kumari

Sharma, for their love and support ever since my childhood. I wish to thank my brother,

Pranav, for all the love and encouragement. My family has been my ultimate support and

I could not have done this without them.

I would like to thank wonderful and inspiring teachers who have taught me over the

years: during my high school, during my undergraduate studies at IIT (esp. Prof. Naresh

Chandiramani), during my internship at INRIA Lorraine (Prof. Francois Charpillet)

and graduate studies here at UFL (esp. Dr. Anil Rao, Prof. Baba Vemuri, Dr. Prabir

Barooah and Prof. Jay Gopalakrishnan). I would also like to thank Shannon Ridgeway for

all the help and many wonderful discussions about robotics and life.

I thank my fellow students at the Center for Intelligent Machines and Robotics. From

them, I learned a great deal and found great friendships. I would also like to thank the

MAE graduate students Nitin Sharma, Shubhendu Bhasin, Ryan Chilton, Jonathon Jeske

and Anubi Moses with whom I have enjoyed exciting discussions. Thanks to Rakesh

Mahadevapuram, Nicole Kanizay, Sreenivas Vedantam, Arindam Banerjee, Mayank

Srivastava, Prashant Anandkrishnan and Vinay Pandey for being such great friends. I also

thank Raghav Aras for all the exciting discussions and inspirational talks. Special thanks

to my dearest, oldest friends Piyush Grover, Praveen Sharma and Rashi Arora.

Finally, I would like to thank the almightly, without whom none of this would have

been possible, for blessing me with such a great opportunity.

4

TABLE OF CONTENTS

page

ACKNOWLEDGMENTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

LIST OF SYMBOLS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

ABSTRACT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

CHAPTER

1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2 THE VESTIBULAR DYNAMIC INCLINOMETER (VDI) . . . . . . . . . . . . 18

2.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.2 Sensor Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.3 Dynamic Equilibrium Axis . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.4 Mathematical Manipulations . . . . . . . . . . . . . . . . . . . . . . . . . . 242.5 Sensor Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.5.1 Control Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 272.5.2 Torque Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282.5.3 Acceleration Control or Postural Throttle . . . . . . . . . . . . . . . 30

2.6 Sensor Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3 ESTIMATING JOINT PARAMETERS USING THE VDI . . . . . . . . . . . . 40

3.1 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403.2 Parameter Measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 413.3 Base Link parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 433.4 Inter-link parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 443.5 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4 THE PLANAR VESTIBULAR DYNAMIC INCLINOMETER (PVDI) . . . . . 52

4.1 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 524.2 Intermediate Coordinate System and Rotation Matricies . . . . . . . . . . 534.3 Sensor Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 554.4 Dynamic Equilibrium Axis for planar motion of the base . . . . . . . . . . 564.5 Kinematic Analysis and Mathematical Manipulations . . . . . . . . . . . . 584.6 Inclination Measurement - Closed form solution . . . . . . . . . . . . . . . 594.7 Sensor Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 614.8 Sensor Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

5

5 ESTIMATING JOINT PARAMETERS USING PVDI . . . . . . . . . . . . . . 66

5.1 Problem Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 665.2 Parameter Measurement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 675.3 Base Link parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 695.4 Inter-link parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

6 CONCLUSION AND FUTURE WORK . . . . . . . . . . . . . . . . . . . . . . 72

APPENDIX: AUTOCALIBRATION OF MEMS ACCELEROMETERS . . . . . . . 74

A.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74A.2 Autocalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75A.3 Misalignment Computation . . . . . . . . . . . . . . . . . . . . . . . . . . 76

REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

BIOGRAPHICAL SKETCH . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

6

LIST OF TABLES

Table page

2-1 Parameters of the system used for simulation . . . . . . . . . . . . . . . . . . . 33

7

LIST OF FIGURES

Figure page

2-1 Model of robot with labeled coordinate systems . . . . . . . . . . . . . . . . . . 19

2-2 Schematic drawing of human vestibular system . . . . . . . . . . . . . . . . . . 19

2-3 Vestibular organ consists of semicircular canals(ducts) and otolith organs . . . . 22

2-4 Detail view of sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

2-5 Mechanics of otolith organs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

2-6 Inclination angle θ vs observed θ . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2-7 Angular acceleration �θ vs observed �θ . . . . . . . . . . . . . . . . . . . . . . . . 26

2-8 Net acceleration experienced by body, ~g vs observed ~g . . . . . . . . . . . . . . 27

2-9 Angular velocity _θ vs observed _θ from gyroscope and angular acceleration . . . . 27

2-10 Angular velocity _θ vs observed _θ from gyroscope and directly from accelerometers 27

2-11 Robot model with motion in known direction as control parameter . . . . . . . . 31

2-12 Control Torque Strategy - the DEA is along vertical . . . . . . . . . . . . . . . . 34

2-13 Acceleration control strategy - the DEA is not along the vertical . . . . . . . . . 35

2-14 Experimental setup of the VDI . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

2-15 Misalignment of linear accelerometer axes . . . . . . . . . . . . . . . . . . . . . 36

2-16 Plot of θ from the VDI vs θ from the encoder when base is static . . . . . . . . 38

2-17 Plot of angular velocity _θ from the VDI vs _θ from the encoder . . . . . . . . . . 38

2-18 Plot of angular acceleration �θ from the VDI vs �θ from the encoder . . . . . . . . 39

2-19 Plot of absolute inclination angle β vs angle from the encoder . . . . . . . . . . 39

3-1 Links i , j joined at point Oi,j having joint angle θi,j . . . . . . . . . . . . . . . . 41

3-2 Location of VDI on a link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

3-3 The VDI sensor function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

3-4 Base link b with one end in contact with ground surface . . . . . . . . . . . . . 43

3-5 Links i , j joined at point Oi,j with joint angle θi,j between them . . . . . . . . . 44

3-6 Slider crank mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

8

3-7 Plot of estimated γ1 vs true γ1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3-8 Plot of estimated Nω1 vs true Nω1 . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3-9 Plot of estimated Nα1 vs true Nα1 . . . . . . . . . . . . . . . . . . . . . . . . . . 47

3-10 Plot of estimated θ12 vs true θ12 . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

3-11 Plot of estimated _θ12 vs true _θ12 . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3-12 Plot of estimated �θ12 vs true �θ12 . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3-13 Plot of estimated joint acceleration NaO12 vs true joint acceleration . . . . . . . 49

3-14 Plot of variation of standard deviation of inclination measured . . . . . . . . . . 50

3-15 Plot of variation of standard deviation of angular velocity . . . . . . . . . . . . 50

3-16 Plot of variation of standard deviation of angular acceleration . . . . . . . . . . 50

3-17 Plot of variation of standard deviation of resultant acceleration . . . . . . . . . 51

4-1 Model of robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

4-2 Definition of Intermediate coordinate system . . . . . . . . . . . . . . . . . . . . 53

4-3 Sensor definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

4-4 Simplified problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

4-5 Plots of angles θ,ψ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4-6 Plots of angular velocities _θ, _ψ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4-7 Plots of angular accelerations �θ, �ψ . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4-8 Plot of estimated ~g vs true ~g . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4-9 Experimental setup of the pVDI . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

4-10 Plot of �θ from the pVDI vs �θ from the encoder. . . . . . . . . . . . . . . . . . . 65

5-1 Links i , j joined at point Oi,j having joint angles θi,j ,ψi,j between them. . . . . . 67

5-2 Location of pVDI on a link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

5-3 The pVDI sensor function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

5-4 Base link b with one end in contact with ground surface . . . . . . . . . . . . . 69

5-5 Links i , j joined at point Oi,j with Euler 1-2 joint angles θi,j , ψi,j between them . 70

A-1 Misalignment of accelerometers . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

9

LIST OF SYMBOLS, NOMENCLATURE, OR ABBREVIATIONS

DEA Dynamic Equilibrium Axis

pVDI planar Vestibular Dynamic Inclinometer

VDI Vestibular Dynamic InclinometerAaB Acceleration of point B in reference frame A

10

Abstract of Dissertation Presented to the Graduate Schoolof the University of Florida in Partial Fulfillment of theRequirements for the Degree of Doctor of Philosophy

VESTIBULAR DYNAMIC INCLINOMETERAND MEASUREMENT OF INCLINATION PARAMETERS

By

Vishesh Vikas

December 2011

Chair: Carl CraneMajor: Mechanical Engineering

A human body displays a remarkable quality of maintaining both static and

dynamic equilibrium for a rigid body in unstable equilibrium (modeled as an inverted

pendulum). Biologically, the sensing of inclination is done by the human vestibular

system. Here, a novel design of a sensor motivated by the human vestibular system is

presented. The sensor, called the Vestibular Dynamic Inclinometer(VDI), measures the

dynamic inclination parameters - inclination angle, angular velocity, angular acceleration

and magnitude of the acceleration of surface of contact (e.g., gravity). The VDI uses

two dual-axis linear MEMS accelerometers and one single axis MEMS gyroscope to

measure the inclination parameters for a four degree-of-freedom robot. The concept of

the Dynamic Equilibrium Axis (DEA) is introduced. The DEA is the axis along which

the robot is at equilibrium. The direction of the DEA is parallel to the direction of the

resultant acceleration of the surface of contact. Two control strategies - torque control and

acceleration control are simulated to explain this concept. The DEA attempts to explain

the reason why humans lean forward while accelerating (sprinting) and backward while

decelerating (sudden stopping). The VDI is used to measure joint parameters - joint angle,

angular velocities and angular accelerations, for links joined by revolute joint. Here, the

VDI is a contactless sensor withe flexible point of application.

The VDI sensor is extended for inclination parameter measurement of five degree-of-freedom

robot. The new sensor is called the planar Vestibular Dynamic Inclinometer (pVDI). The

11

pVDI consists of four dual-axis linear MEMS accelerometers and one tri-axial MEMS

gryoscope. The orientation of the linear accelerometers is different from the intuitive

analogous VDI. This is due to the coupling in the kinematics. Similar to the VDI, the

pVDI is used to measure joint parameters with links joined by universal hooke joint. The

concept of the DEA is also extended.

The novelty of the VDI and the pVDI lies in the fact that the measurements are

independent of drift or integration errors, acceleration of surface of contact (e.g.,

gravitational acceleration), independent of the dynamics of the robot, and require

significantly less computational burden (closed form solution). The inclination angle

obtained from the VDI and the pVDI is from the DEA. As the goal of balancing tasks is

to bring the robot to equilibrium, the inclination parameters obtained from the VDI and

the pVDI are suitable candidates for control applications. The application of the VDI and

the pVDI is foreseen in gait analysis, industrial robotics, humanoid robotics, etc.

12

CHAPTER 1INTRODUCTION

The human body has been a source of motivation for mechanical design. It

has inspired designs of a large number of sensors e.g., vision, stereo vision, haptics,

etc. Mechanically, the human body displays a remarkable quality of maintaining

static equilibrium for a body that is in a state of unstable equilibrium (biped stance).

Biologically, the sensing of inclination of the human body is done using the vestibular

system. It provides feedback to maintain the body in an equilibrium position at all times.

The human body is able to maintain equilibrium even when gravity changes (e.g., the

moon, etc) and when the surface of contact is accelerating (e.g., an accelerating bus).

Interestingly, in such circumstances the equilibrium position of the body changes e.g.,

leaning forward while sprinting and leaning backward while trying to stop.

Inertial sensors are aimed at measuring acceleration. There two types of inertial

sensors - accelerometers and gyroscopes. Since the widespread availability of micro

electromechanical (MEMS) sensors, inertial sensors have been widely used in amubulatory

systems to study human movement, navigation and estimate joint parameters. Applications

include gait analysis [66], [77], [76], [46], [47], [48], [52], research in motor and control

stability [50], load estimation [69] and monitoring activities of daily living (ADL) and

level of activity [12], [71]. For these applications, estimation of the orientation is essential

e.g., for load estimation using inverse dynamics using accelerometers and gyroscopes, the

orientation and angular velocity of a segment need to be known.

In the absence of non-gravity acceleration, a tri-axial accelerometer can be used as

an inclinometer [76], [37],[31], [8]. These days, a widely common use is in smartphones

which use this to switch between landscape and portrait mode. For this static case, the

algorithm measures the angle between the sensor unit with respect to the gravity. This

will be less accurate for relatively large non-gravity accelerations. Furthermore, linear

acceleration does not give complete information about the orientation.

13

Gyroscopes are used to measure angular velocity. Strapdown integration algorithms

[28], [11], [29] calculate the change in orientation by integrating the angular velocity.

The word strapdown indicates that the angular velocity is obtained from the gyroscope

strapped onto an object. However, small errors in angular acceleration (gyroscope signal)

will give rise to large integration errors. Moreover, for measuring absolute orientation and

not change in orientation, a reference orientation has to be set.

Orientation estimation is also done by fusion of accelerometer and gyroscope data.

Such sensors are called Inertial Measurement Units (IMUs). IMUs are used in field

robotics [5], assessment of human balancing [6], space navigation [33], etc. The usual

practice is to use three single-axis accelerometers and three single-axis gyroscopes aligned

orthogonally [33], [68]. A Kalman filter [30], [60] with knowledge of (error) dynamics

of the system is applied to minimize these errors, emphasizing the correctness of linear

accelerations when angular accelerations are low, and trusting the gyroscope data more

when the motion is more dynamic [39], [38], [61], [3], [51], [1], [36]. A filter for estimating

the orientation of human body segments has been researched [2]. Here the filter used

the accelerometer and magnetometer readings to obtain the low frequency component of

the orientation and used gyroscope for measuring the faster changes in the orientation.

This may be problematic as the use of a magnetometer in the vicinity of ferromagnetic

materials will give large errors. Another sensor unit [22] containing a dual-axis fluid

inclinometer, a dual-axis electronic compass and tri-axial gyroscope, with a Kalman filter

that incorporated continuous gyroscope offset estimate has been researched.

Many different principles for inclination estimation are commonly used [10], [80],

[35]. Other approaches involve tracking the movement of the pendulum using conductive,

resistive or capacitive measurement systems [7], [56]. Gyroscope-free designs using

only accelerometers have been explored to detect inertial sensing - angular velocity

and linear acceleration [62], [15]. However, in all the approaches, the integration errors

accumulate. Robots from commercial companies (e.g., Sony, Honda) use gyroscopes

14

and/or accelerometers in their active balancing, but details are not provided by the

creators and quantitative comparison to humans are not available.

In humans, the balancing mechanism is based on visual and vestibular feedback to

maintain the body in an unstable equilibrium biped stance. The human body enjoys

stability even during lack of visual feedback and a well known cause of instability is

failure of vestibular sensors [13]. This clearly indicates the importance of the vestibular

system for human stance stabilization. Biomechanics of the vestibular system has been

investigated in detail [57]. The human vestibular system-sensor analogy is always drawn

as single gyroscope and single accelerometer based or using an additional sensor like

magnetometer a [44], [65], [39], [38], [61], [70], [53], [79]. A completely different analogy is

drawn for measurement of inclination parameters for a body modeled as a planar inverted

pendulum. Here, two dual-axis linear accelerometers and a single-axis gyroscope are used

as sensors for parameter measurement. The symmetric design of the ears motivated the

placement of the accelerometers - symmetrically placed across a vertical line. This novel

sensor is called the Vestibular Dynamic Inclinometer (VDI). The proposed sensors are

MEMS accelerometers and gyroscopes [9].

Humanoid robots require to maintain dynamic balance during a walk. The Zero

Moment Point (ZMP) [73], a concept used for equilibrium analysis and motion planning

of biped locomotion has been extensively researched. The ZMP plays and important role

for gait analysis, synthesis, and control. However, the humanoid robots run at maximum

speed of 7 km/h (Toyota’s humanoid, Boston Dynamics’ Petman) or 6 km/h (Honda’s

Asimo). The concept of the Dynamic Equilibrium Axis (DEA) related to the dynamic

equilibrium of robots is introduced. The DEA is foreseen to be helpful in the theoretical

consideration and practical development of fast speed running robots.

Studies show that contributions from visual, vestibular, joint angle proprioceptive,

and force help in human stance control [27]. Vision improves stance stability, but in

principle, can be dispensed with [45]. Joint torque and ground force sensors can be applied

15

for sensing force-related information. Inclination parameters - inclination angle, angular

velocity, and angular acceleration are also needed for closed-loop feedback control of

manipulators, humanoid robots, etc. The conventional joint angle measurement sensors

are magnetic rotary encoders and optical rotary encoders [16]. Magnetic encoders are

low-cost, contactless and reliable but require special magnet coupling alignment and

magnetic shielding. Optical encoders are very accurate and contactless, but are expensive

and are sensitive to environmental influences (shock, vibration, etc.). These sensors

require installment at the joint center which may be problematic (or impossible) for

some applications, e.g., human knee joints. Other contactless joint angular measurement

sensors use microelectromechanical system (MEMS) accelerometers and gyroscopes [9].

Unlike the conventional sensors, they do not require tight coupling to relative mechanical

movements, and thus are more flexible at the point of installation, more reliable, and last

longer. The common-mode-rejection (CMR) method [34], [24], [25], [77] uses two dual-axis

accelerometers mounted on adjacent links, ideally attached to the joint center. This

method displays large errors for rapid rotation. Non-ideal placement of the accelerometers

also leads to errors. To deal with this, the following three methods [16] - CMR with

gyro-integration (CMRGI), CMR with gyro-differentiation (CMRGD) and distributed

CMR (DCMR) were introduced. The CMRGI and CMRGD use one dual-axis linear

accelerometer and one single-axis gyroscope per link for joint angle estimation [16].

The CMRGI [77], [41] integrates the gyroscope signal to obtain a change in orientation.

This method faces problems due to integration error/drift due to noise. The CMRGD

[21] differentiates the gyroscope signal to obtain angular acceleration. It uses the

angular acceleration and angular velocity to obtain the acceleration of the joint, then

calculates the joint angle. Due to noise, the differentiation of angular velocity to angular

acceleration has undesirable errors. The DCMR method [16], [75] uses two dual-axis linear

accelerometers (not symmetrically placed) per link to estimate the joint angle. Here, the

difference of the two acceleration signals per link is used to determine the joint angle, thus,

16

avoiding the errors faced by other CMR methods. A novel way of using the VDI sensors,

one on each link, to measure the inclination, angular velocity, and angular acceleration is

presented. Measuring the inclination parameters for the base link (link in contact with the

ground) is also discussed.

The human body can be visualized as a five degrees-of-freedom inverted pendulum.

The VDI is extended to inclination measurement of five degrees-of-freedom robots. The

modified sensor design is termed as the planar Vestibular Dynamic Inclinometer(pVDI)

and differs from the VDI in sensor orientation. The concept of the DEA is also extended

here. The application of the pVDI to measure inclination parameters between two links

joined by a universal joint is presented.

The inclination angle obtained from the VDI and the pVDI is relative to the Dynamic

Equilibrium Axis (DEA), the equilibrium axis/position. As the inclination is relative

to an equilibrium point, it is foreseen as a more suitable control feedback candidate for

robot balancing and stabilizing. The efficacy of the sensors comes from the fact that

they are simple, low-cost, require low computation, and provide the dynamic inclination

angle directly, without requiring to integrate the angular velocity and applying a Kalman

filter for sensor fusion. Also, the angular calculations are valid for large angles and are

independent of the acceleration of surface of contact (gravity, etc.). This makes the sensor

very useful for robot balancing in varying gravitational environments (space applications)

and accelerating platforms (running, walking motion of robots, etc.). Applications of the

VDI and pVDI to measure link parameters in linkage mechanisms makes them useful for

gait analysis, biped robots, etc.

17

CHAPTER 2THE VESTIBULAR DYNAMIC INCLINOMETER (VDI)

2.1 Problem Formulation

A robot is modeled as an inverted pendulum as shown in Figure 2-1. It is desired

to sense the inclination and angular velocity of the robot independent of acceleration of

surface of contact (point O). The rigid body is modeled as a rod with mass m, center of

mass C along the rod at a distance rC from the base of the rod, moment of inertia IBC at

point C and angular damping coefficient Kd . Let point O be the base of the rigid body in

contact with a platform. Let N represent the inertial reference frame and B represent the

reference frame fixed on the rigid body. Let g be the gravitational acceleration, a be the

acceleration of point O with respect to the earth reference frame and ~g be the resultant of

the previous two mentioned accelerations. Thus~g = g+ a (2–1)

A coordinate system fixed in the inertial reference frame with origin at O, Y-axis parallel

to the ground, Z-axis into the plane of the paper, and fixed in reference frame N is

defined with {x, y, z} as the orthonormal basis. Another coordinate system with originO fixed in the body reference frame B, with {er, eθ, ez} orthogonal basis vectors in the

radial, tangential directions, and into the plane of the paper respectively, is defined. The

Dynamic Equilibrium Coordinate system is defined to be fixed in the inertial reference

frame N with origin at point O. The vectors {ex, ey, ez} form a set of orthonormal basis

vectors such that ex is parallel to vector ~g and ez is into the plane of the paper. Let φ be

the angle between g and ~g in a clockwise direction. Let θ be the angle between ~g and rigid

body, as shown in Figure 2-1. The angular velocity between coordinate system N and B is

denoted by NωB which may be written asNωB = _θez (2–2)

18

O

P

C

B

er

^

eθ

^

rC l

L

R

ex

ey

^

^

G

θφ

φ

NX

Ya

gg~

Figure 2-1. Model of robot with labeled coordinate systems in different reference frames.Note that point O may also experience non-gravity acceleration (a)

Figure 2-2. Schematic drawing of human vestibular system

Point P is at a distance l radially along the body, whereas, the accelerometers R, L are

located at a distance d/2 on either side of point P in a tangential direction of the body as

shown in Figure 2-4. Let NaL/R denote the acceleration sensed by the L/R accelerometer.

2.2 Sensor Design

For measurement of the spatial orientation of the body, humans possess vestibular

organs (Figure 2-2), the equivalent biological inertial measurement unit. The vestibular

organs lie in the inner ear, are encapsulated by bone, thus, are affected only by force fields

such as gravitational force fields [44], [78]. They consist of two main receptor systems [42]

19

for inertial sensing - semicircular canals(ducts) and otolith organs. The receptor cells of

the otoliths and semicircular canals send signals through the vestibular nerve fibers to the

neural structures that control eye movements, posture, and balance.

The semicircular canals are the primary systems responsible for sensing angular

acceleration of the head and transmitting the information to the brain stem. Each canal

is comprised of a circular tube containing fluid continuity, interrupted at the ampulla

(that contains the sensory epithelium) by a water tight, elastic membrane called the

cupula. The three semicircular canals, arranged in orthogonal planes, take advantage of

endolymph fluid dynamics to sense angular motion. Angular motion sensation relies on

inertial forces, caused by head accelerations, to generate endolymph fluid flow within the

toroidal semicircular canals [14]. The site of mechanotransduction within the semicircular

canals is localized to the crista ampullaris, which is a crestlike ridge in the ampullary wall

that protrudes into the lumen of the ampulla. A sensory epithelium resides on the surface

of the crista, which is encased by the cupula (Figure 2-3A). During rotation (but not

translation), fluid inertia exerts pressure on neural receptors and leads to a neural signal.

Due to mechanical factors (such as fluid adhesion), the canals code angular acceleration

during low frequency rotation and angular velocity in the mid- to high-frequency range

[44] as illustrated in Figure 2-3A. The angular velocity signal from the semicircular canals

contain noise, that, after velocity-position integration, have fluctuating drifts [43], [42].

Research has been done to build macromechanical model of the semicircular canals [20],

[19], [58]. Here, considering the reliability of microelectromechanical sensors motivates

us to assume the semicircular canals to be analogous to a MEMS gyroscope sensor.

The MEMS gyroscope signal also contains noise, but, is not integrated to obtain the

inclination.

The otolith organs comprise of utricle and saccule of the inner ear. The otolith organs

are flat layered structures that lie between the endolymphatic fluid and the membranous

labyrinth substrate (Figure 2-3B). The biomechanical response of the otolith organs is

20

critical to the ability to sense the direction of gravito-inertial acceleration. The utricle is

sensitive to a change in horizontal linear acceleration and the saccule is sensitive to the

vertical linear acceleration [57]. Here, a dual-axis accelerometer (MEMS) is assumed to be

analogous to the otolith organs.

Each vestibular organ is, thus, assumed to be analogous to a dual-axis accelerometer

and a single-axis gyroscope. Human ears are places symmetrically about the axis of

symmetry (along which the nose lies), thus, providing motivation to design a sensor

that has vestibular-analogous accelerometer-gyroscope symmetrically placed about a

symmetrical line. The gyroscope readings for both the gyroscopes will be theoretically

the same (as they are attached to the same rigid body). However, the accelerometer

readings for both accelerometers will be different. The design in Figure 2-4 is proposed

which has two dual-axis accelerometers (L, R) symmetrically placed across a vertical

line and one single-axis gyroscope (G ). This sensor is called the Vestibular Dynamic

Inclinometer(VDI). Throughout the literature, the human vestibular system-sensor

analogy is drawn as a single gyroscope and single accelerometer. As the linear accelerations

in each of the vestibular organs are different, such analogy leads to loss of critical

information.

The vestibular system is capable of sensing the angular acceleration from the

difference of the linear acceleration readings of the left and right vertical otoliths

(Figure 2-5A). The difference between left and right horizontal otoliths can also sense

the magnitude of the angular velocity but is not able to determine the direction (Figure

2-5B). Further analysis will confirm that the difference between horizontal (or radial)

otoliths (accelerometer readings) yield angular acceleration. Also, the difference between

vertical (or tangential) otoliths (accelerometer readings) yields magnitude, and not

direction of angular velocity.

21

(A) Angular accelerationcauses viscous flowof endolymphthrough thesemicircular canaland into theampulla, deflectingthe cupula toregister accumulatedangular velocity

(B) Damped by fluidendolymph, otolithorgans deflect fromequilibrium positionto register linear

acceleration

Figure 2-3. Vestibular organ consists of semicircular canals(ducts) and otolith organs

P

B

er

eθ

^

^

G

d/2

L R

d/2

Figure 2-4. Detail view of sensor - accelerometers R, L and gyroscope G2.3 Dynamic Equilibrium Axis

At any point M on the body, performing a kinematic analysis [59] to obtain

acceleration (NaM) yieldsNaM = NaO + NαB × rO→M + NωB × (NωB × rO→M) (2–3)

22

(A) The difference between corresponding leftand right saccule registers angular

acceleration.

(B) The difference between corresponding leftand right utricle can register themagnitude of angular velocity but notthe direction.

Figure 2-5. Mechanics of otolith organs

where M may be {L,R,C} and NαB is the angular velocity between coordinate system Nand B. It is known that rO→C = rC errO→L/R = l er ∓ d2 eθNaO = ~g = ~gex = −~g os θer + ~g sin θeθ (2–4)

where NaO denotes the acceleration sensed by the accelerometer when it is placed at pointO. Therefore, NaC = ~g+ rC (�θeθ − _θ2er) (2–5)NaL = −(l _θ2 − d2 �θ − ~g os θ) er + (l �θ + d2 _θ2 − ~g sin θ) eθ (2–6)NaR = −(l _θ2 + d2 �θ − ~g os θ) er + (l �θ − d2 _θ2 − ~g sin θ) eθ. (2–7)

Let the reaction forces at point O be FR . Application of Euler’s first and second law [59]

about the center of mass C givesm (~g+ rC (�θeθ − _θ2er)) = FR. (2–8)IBC · NαC = −Kd _θez + (−rC er)× FR. (2–9)

23

Thus,(IBC +mr2C) �θ = −Kd _θ +m~grC sin θ (2–10)

From Equation 2–10, the equilibrium position (�θ∗ = 0, _θ∗ = 0) of the system is θ∗ = 0.The aim of the problem is to bring the body to its equilibrium position which is no

more the absolute vertical position (i.e., direction parallel to the gravity vector g). The

new equilibrium position is defined as the Dynamic Equilibrium Axis (DAE) which is

parallel to the resulting acceleration on the body ~g, inclined at angle φ to g. The Dynamic

Equilibrium Axis is dependent on the resultant acceleration acting on the body, and thus

is time-varying (more precisely, acceleration varying). When the body is accelerating, φ is

positive, when its de-accelerating, φ is negative. This fact can be observed when humans

lean forward (change equilibrium axis) when trying to accelerate (sprint) and bend

backwards while attempting to de-accelerate. Both the cases display how the equilibrium

axis (DEA) changes when acceleration is experienced by the body.

2.4 Mathematical Manipulations

In Equations 2–6 and 2–7 the kinematic analysis of the body is performed. It is

important to observe that all the calculations are independent of the dynamics of the

body, i.e., control torque, external force, etc. Four readings are obtained from the two

accelerometers (radial and tangential components in Equations 2–6, 2–7). Calculating the

difference and mean of the readings and calling them ζ1, ζ2 yields

ζ1 = FaL − FaR (2–11)

ζ2 = 12 (FaL + FaR) (2–12)

ζ1 = d �θer + d _θ2eθ (2–13)

ζ2 = − (l _θ2 − ~g os θ) er + (l �θ − ~g sin θ) eθ (2–14)

24

ζir , ζiθ represent the radial (er) and tangential (eθ) components of ζi , where, i = 1, 2. Thus,

the angular acceleration, velocity and inclination can be obtained as�θ = ζ1rd (2–15)sin θ = ld ζ1r − ζ2θg (2–16) os θ = ld ζ1θ + ζ2rg (2–17)tan θ = ld ζ1r − ζ2θld ζ1θ + ζ2r (2–18)~g = ld ζ1r − ζ2θsin θ = ld ζ1θ + ζ2r os θ . (2–19)

It is important to observe that tan θ (Equation 2–18) is independent of the resultant

acceleration, ~g, of surface of contact. Inclination θ can be uniquely determined as ~g > 0.It is also possible to determine the resulting acceleration magnitude ~g from Equation 2–19.

Obtaining the angular velocity ( _θ) is a little tricky, as the sensor mathematics is able to

obtain the magnitude of the angular acceleration, but not the direction. Angular velocity

can be obtained by integration of angular acceleration (Equation 2–20), directly from

accelerometers (Equation 2–21) or from the gyroscope readings (Equation 2–2)._θI = ∫ �θdt (2–20)_θotolith = sign(∫ �θdt)√

∣

∣

∣

∣

ζ1θd ∣

∣

∣

∣

(2–21)

Calculation of angular velocity from Equation 2–20 is prone to accumulation of error

(drift) when the body is in state of equilibrium and Equation 2–21 will result in

magnification of contribution of noise as a square root is involved. For all these reasons,

the reading from the gyroscope is used to calculate the angular velocity.

2.5 Sensor Simulation

Simultaneous analysis of sensor simulation and experiment is performed. Similar

results from both analysis will confirm the accuracy of the simulation model and allow

25

0 0.5 1 1.5 2 2.5 3 3.5 4−80

−60

−40

−20

0

20

40

60

time

degrees

θ vs observed θ

θ

observed θ

Figure 2-6. Inclination angle θ vs observed θ

0 0.5 1 1.5 2 2.5 3 3.5 4−80

−60

−40

−20

0

20

40

60

80

time

degrees/sec2

Angular Acceleration vs Observed Angular Acceleration

Angular Acceleration

Observed Angular Acceleration

Figure 2-7. Angular acceleration �θ vs observed �θfurther analysis using the simulation model. Simulation analysis of the above stated

approach is performed in MATLAB R©. The parameters for simulations are taken to be

analogous to the human body with l = 2m, d = 0.25m. The sensor noise was modeled

as stationary white noise which is constant throughout the frequency spectrum based on

the specification data sheet of accelerometer ADXL213 (noise density of 160 µg/√Hz rms)

and gyroscope ADXRS450 (noise density of 0.015 o/se /√Hz) which are manufactured

by the company Analog Devices. For future simulations, sensor noise is modeled in a

similar fashion. Figure 2-6 shows good estimation of inclination angle θ purely from

kinematic calculations and without the use of system dynamics. The angular acceleration

and magnitude of net acceleration acting on the body are also well estimated as shown

in Figure 2-7 and 2-8. Figures 2-10, 2-9 prove why calculation of the angular velocity

from Equations 2–20, 2–21 is erroneous. Figure 2-10 compares the angular velocity

estimated using Equation 2–21 to that from gyroscope. Figure 2-9 shows the concept of

error accumulation (drift) due to integration when the angular velocity is calculated using

Equation 2–20.

26

0 0.5 1 1.5 2 2.5 3 3.5 48

10

12

14

16

18

20

22

24

26

28

time

m/sec2

Resultant acceleration on body

Net Acceleration on body

Observed Net Acceleration

Figure 2-8. Net acceleration experienced by body, ~g vs observed ~g0 0.5 1 1.5 2 2.5 3 3.5 4

−1

0

1

time

degrees/sec

Angular Velocity from Gyroscope

Angular Velocity

Observed Angular Velocity

0 0.5 1 1.5 2 2.5 3 3.5 4−1

0

1

time

degrees/sec

Angular Velocity from Integration

Angular Velocity

Observed Angular Velocity

Figure 2-9. Angular velocity _θ vs observed _θ from gyroscope and observed _θ fromEquation 2–20. Accumulation of error (or drift) can be viewed

2.5.1 Control Strategy

To analyze the concept of the DEA, it is proposed to design a control strategy to

bring the body to equilibrium. The control strategies can be torque application (analogous

to torque on human body for equilibrium via hip, ankle) or acceleration/de-acceleration of

the body (analogous to the leaning of the human body while accelerating or de-accelerating).

The former is referred to as torque control and the latter as acceleration control or

postural throttling.

0 0.5 1 1.5 2 2.5 3 3.5 4−100

0

100

time

degrees/sec

Angular Velocity from Gyroscope

Angular Velocity

Observed Angular Velocity

0 0.5 1 1.5 2 2.5 3 3.5 4−100

0

100

time

degrees/sec

Angular Velocity from Accelerometers

Angular Velocity

Observed Angular Velocity

Figure 2-10. Angular velocity _θ vs observed _θ from gyroscope and observed _θ fromEquation 2–21. It can be observed that the error is magnified

27

It is desired to design a controller for varying weight, unknown moment of inertia,

and damping coefficient. For these reasons, Lyaponov based non-linear controllers are

proposed. The goal is to bring the rigid body back to equilibrium. To achieve the control

purpose, a tracking error e ∈ R is defined ase , θd − θ = −θ (2–22)

where the desired angle of inclination θd ∈ R is zero for all time. For stability analysis of

the system, filtered tracking error r ∈ R is defined asr , _e + αe = − _θ − αθ (2–23)

where α ∈ R is a positive real constant.

2.5.2 Torque Control

In this case, the torque is applied on the body and the equations of motion change

from Equation 2–10 to IBO �θ + Kd _θ +m~grC sin θ = TC (2–24)

where IBO = (IBC +mr2C) and TC is the control torque. Y ∈ R1×3 is defined asY ,

[ _θ, ~g sin θ,−α _θ] . (2–25)

The control torque is designed as TC = Y �− kr (2–26)

where k ∈ R is a positive constant and the update law for � ∈ R3×1 is_� = �Y T r (2–27)

where � ∈ R3×3 positive definite adaptation gain matrix and α, k are constrained as

follows

α, k > 12. (2–28)

28

Theorem 2.1. The controller given in Equations 2–26 and 2–27 with gain conditions given

in Equation 2–28 ensures that the tracking error is regulated as followslimt→∞||e(t)|| = 0, limt→∞

|| _e(t)|| = 0thus, assuring global asymptotic stability.

Proof. Differentiating the filtered tracking error yields_r = �e + α _e = −�θ − α _θ. (2–29)

Multiplying Equation 2–29 by IBO and simplifying using Equation 2–24IBO _r = Kd _θ +m~gr sin θ − IBOα _θ − TC (2–30)IBO _r = Y�− TC (2–31)

where � ∈ R3×1 is unknown, yet deterministic and defined as� ,

[Kd ,m, IBO ]T . (2–32)~� ∈ R3×1 is defined as ~� = �− �. (2–33)

Combining Equation 2–26, 2–31, 2–33 givesIBO _r = Y ~�− kr . (2–34)

Let V denote a continuously differentiable positive definite radially unbounded Lyapunov

function candidate defined asV ,12 IBO r2 + 12e2 + 12 ~�T�−1 ~�. (2–35)

Differentiating 2–35 and simplifying yields_V = −kr2 − αe2 + er + Y ~�r + ~�T�−1 _~� (2–36)

29

using the Arithmetic Mean-Geometric Mean inequalityer ≤ r2 + e22 . (2–37)� is unknown and deterministic. Thus, _~� = − _�. Simplifying Equation 2–36 using 2–37

and 2–27 gives _V ≤ −(k − 12) r2 −(

α− 12) e2. (2–38)

Using the constraints given in Equation 2–28, the expression in Equation 2–38 is upper

bounded by a continuous, negative semi-definite function. By using Barbalat’s Lemma [32]limt→∞||e(t)|| = 0, limt→∞

|| _e(t)|| = 0 ∀θ ∈ R. (2–39)

2.5.3 Acceleration Control or Postural Throttle

In this case, the control and motion looks as shown in Figure 2-11. The control

of the body is a force in a specific direction (y), and thus it is safe to assume that the

acceleration in the other direction (x) is known/calibrated (usually gravitational). Thus,~g = gx+ ahy. (2–40)

Let β = (θ + φ), be the inclination of the body relative to the absolute gravity g. Now,

the equations of motion change toIBO �θ +mrC os βah + Kd _θ −mrC sin βg = 0 (2–41)mrC os β�θ +mah −mrC sin β _θ2 − FC = 0 (2–42)

φ = tan−1(ahg ) = os−1(g~g) (2–43)

where FC is the control force and ~g = √g2 + a2h. Defining Y ∈ R1×4 asY ,

[ _θ,−α _θ,−gsβ, (( _θ + r) β − αsβ) sβ _θ] . (2–44)

30

O

C

B

NX

Yah

g

FD F

C

Figure 2-11. Motion in known direction as control parameter. Here, β = φ+ θ, inclinationof the body from the absolute gravity g.

The control force is designed as FC = − 1 os β (Y � + kr) (2–45)

where k ∈ R is a positive constant and the update law for � ∈ R4×1 is_� = �Y T r (2–46)

where � ∈ R4×4 is a positive definite adaptation gain matrix and α, k are constrained as

follows

α, k > 12. (2–47)

It is worthwhile to mention that the system becomes uncontrollable when β = π/2, i.e.,when the rigid body is parallel to the direction of acceleration. Therefore, os β > 0 for all

controllable cases.

Theorem 2.2. The controller given in Equations 2–45 and 2–46 with gain conditions given

in Equation 2–47 ensures that the tracking error is regulated as followslimt→∞||e(t)|| = 0, limt→∞

|| _e(t)|| = 0thus, assuring global asymptotic stability.

31

Proof. The proof proceeds on similar lines as the proof for the previous theorem. Solving

Equations 2–41, 2–42 for �θ and ah givesA�θ = −Kd _θ −mr2 sβ β _θ2 +mgr sβ − rC βFC (2–48)Aah = IBOmFC + IBO rC sβ _θ2 + Kd rC β _θ −mr2Csβ βg (2–49)

where A = (IBC +mr2Cs2β). It can be observed that A > 0∀β. Calculating the following

expression using the expression for the filtered tracking error as defined in Equation 2–23

gives A_r + _ArrC = KdrC _θ − IBCrC α _θ −mgsβ + βFC+mrC (( _θ + r) β − αsβ) sβ _θA_r + _ArrC = Y�+ βFC (2–50)

where � ∈ R4×1 is unknown, yet deterministic and is defined as� = [KdrC , IBCrC ,m,mrC]T . (2–51)~� ∈ R

4×1 is defined as ~� = �− �. (2–52)

Combining Equations 2–45, 2–50, 2–52 givesA_r + _ArrC = Y ~�− kr . (2–53)

Let V denote a continuously differentiable positive definite radially unbounded Lyapunov

function candidate defined asV ,12rC Ar2 + 12e2 + 12 ~�T�−1 ~�. (2–54)

Differentiating 2–54, using Equation 2–53 and simplifying yields_V = −kr2 − αe2 + er + Y ~�r + ~�T�−1 _~�. (2–55)

32

As � is unknown and deterministic, _~� = − _�. Simplifying Equation 2–55 using 2–37 and

2–46 gives _V ≤ −(k − 12) r2 −(

α− 12) e2. (2–56)

Using the constraints given in Equation 2–47, the expression in Equation 2–56 is upper

bounded a by continuous, negative semi-definite function. By using Barbalat’s Lemma [32]limt→∞||e(t)|| = 0, limt→∞

|| _e(t)|| = 0 ∀θ ∈ R. (2–57)

Simulations were performed in MATLAB R©. The sensor noise was modeled as

explained in Section 2.5. The frequency of operation of the sensors is assumed to be10 Hz . Values of other parameters are motivated by modeling of the human body as

an inverted pendulum. The values for simulation are displayed in Table 2-1. For the

Parameter Value Parameter Valuem 85 kg l 2 md 0.25 m L 1.8 mR 1.25 m IO mL2/3Kd 2 N m s rad−1 g 9.81 m/s2Table 2-1. Parameters of the system used for simulation

simulation of the torque control strategy, the gains are assumed to be α = 4, k =2, � = 103I3×3. The learning of parameters is one online. Angular response and control

torque comparisons for different initial inclinations are shown in Figure 2-12A and 2-12B

respectively. It is important to observe in Figure 2-12A that the rigid body balances to

± 0.5o in a relatively quick time and remains in that vicinity thereafter. Also, the torque

required to maintain the rigid body at that inclination is relatively small when the foot

is modeled as a point. In real life, the foot is not a point of contact, rather a surface of

contact, thus assuring better balance. The peak torque generated is a little high but the

results are very encouraging.

33

0 1 2 3 4 5 6 7 8 9 10−1

0

1

2

3

4

5

6

7

8

Time (sec)

Angle (degrees)

Angle Response over time

θ(0) = 7o

θ(0) = 8o

θ(0) = 9o

θ(0) = 10o

(A) Angular Response for differentinitial inclinations, zero initialvelocity( _θ(0) = 0) 0 1 2 3 4 5 6 7 8 9 10

−600

−400

−200

0

200

400

600

800

Time (sec)

Torque (Nm)

Control Torque over time

θ(0) = 7o

θ(0) = 8o

θ(0) = 9o

θ(0) = 10o

(B) Control Torque for different initialinclinations, zero initialvelocity( _θ(0) = 0)

Figure 2-12. Control Torque Strategy - the DEA is along vertical

For the simulation of the acceleration control strategy, the gains are assumed as

α = 1, k = 4, � = 25I4×4. Angular response (β, not θ, as β is the absolute position of the

body), control force and horizontal acceleration comparisons for different initial inclination

are shown in Figures 2-13A, 2-13B and 2-13C respectively. The horizontal acceleration

increases accordingly to balance the robot at the desired DEA. The peak control force

seems higher than normal (gait forces for a 85 kg human), but, the transient response

looks good in a relatively large neighborhood. For the torque control strategy, the DEA

is aligned with the vertical (along g). However, for the acceleration control strategy, the

DEA is not aligned with the vertical as the acceleration acting on the body is no more the

gravitational acceleration. This simulation helps to explain the leaning forward of athletes

when they sprint/accelerate.

2.6 Sensor Experiment

The experiment was set with two dual-axis linear MEMS accelerometers (ADXL320)

symmetrically placed about the center-line of the inverted pendulum as shown in

Figure 2-14. The single-axis MEMS gyroscope (ADRS613) was also strapped onto the

inverted pendulum as shown in Figure 2-14. A linear MEMS accelerometer (ADXL320)

was fixed to the base of the inverted pendulum to sense the acceleration of the base

(Figure 2-14). Interfacing of the analog voltage signal was done using NI-DAQmx and

34

0 2 4 6 8 10 12 14 16 18 2026

28

30

32

34

36

38

40

42

44

Time (sec)

β (d

eg

ree

s)

β Response over time

θ(0) = 15o

θ(0) = 20o

θ(0) = 25o

θ(0) = 30o

(β=θ+φ)Absolute angle

(A) Angular Response(β) for differentinitial inclinations, zero initialvelocity( _θ(0) = 0) 0 2 4 6 8 10 12 14 16 18 20

0

200

400

600

800

1000

1200

1400

1600

1800

Time (sec)

Fo

rce

(N

)

Control Force over time

θ(0) = 15o

θ(0) = 20o

θ(0) = 25o

θ(0) = 30o

(B) Control Force for different initialinclinations, zero initialvelocity( _θ(0) = 0)

0 2 4 6 8 10 12 14 16 18 20−5

0

5

10

15

20

25

30

Time (sec)

a h (m/sec2)

Horizontal Acceleration over time

θ(0) = 15o

θ(0) = 20o

θ(0) = 25o

θ(0) = 30o

(C) Horizontal Acceleration(ah)response for different initialinclinations, zero initialvelocity( _θ(0) = 0)

Figure 2-13. Acceleration control strategy - the DEA is not along the vertical

LabVIEW R©. Magnetic encoders (US Digital MA3-A10-125-N) were fixed at the joints.

The measurement of the encoder angle was assumed to be the ground truth.

Symmetric placement of the linear accelerometers was easily achieved. However,

the alignment of the accelerometers to the ‘theoretical’ radial and tangential directions

in the body coordinate system (er, eθ) was a challenge. To compensate for the coupling

due to mis-alignment, assuming linearity of the accelerometer, the voltage-acceleration

relationship for a particular accelerometer can be modeled as follows

NarNaθ

=

x1 x2x3 x4

vrvθ

+

x5x6

(2–58)

35

MEMS Linear Accelerometers

MEMS Gyroscope

25 cm

Magnetic

Encoder

MEMS Accelerometer

(Base Acceleration)

56 cm

Figure 2-14. Experimental setup of the Vestibular Dynamic Inclinometer(VDI). The twosymmetrically placed linear MEMS accelerometers are marked in boxes. TheMEMS gyroscope is marked by circle. The magnetic encoder and anaccelerometer fixed to the base are indicated by arrows.

P

BNa

Rr

NaLr

NaLθ

NaRθv

Lθ

vRθ

vRr

vLr

eθ

er

^

^

G

d/2

L R

d/2

Figure 2-15. The misalignment of linear accelerometer axes is shown. It is desired toobtain the accelerations in the er, eθ directions - NaLr , NaRr , NaLθ, NaRθ. Thelinear accelerometers voltage signals read linear accelerations in themisaligned axes (dotted lines) - vLr , vRr , vLθ, vRθ

36

where Nar , Naθ represent the radial, tangential components of the acceleration of the

linear accelerometer in the body coordinate system (er, eθ) and vr , vθ represent voltagereadings of the linear accelerometer corresponding to acceleration components in the

misaligned axes as shown in Figure 2-15. It is desired to calibrate for (or find) X6×1 =[x1, x2, x3, x4, x5, x6℄T . Rewriting Equation 2–58

vr vθ 0 0 1 00 0 vr vθ 0 1

X =

NarNaθ

(2–59)

Let vr ,i , vθ,i , Nar ,i , Naθ,i be the set of voltage and acceleration signals for sample/reading i .For m samples/readings

V2m×6 =

vr ,1 vθ,1 0 0 1 0...

......

......

...vr ,m vθ,m 0 0 1 00 0 vr ,1 vθ,1 0 1...

......

......

...0 0 vr ,m vθ,m 0 1

A2m×1 =

Nar ,1...Nar ,mNaθ,1...Naθ,m

(2–60)

It is desired to solve for X from the following linear equationV X = A (2–61)

The least squares solution to the equation can be obtained using Normal Equations,

QR Factorization, or Singular Value Decomposition(SVD) [67]. For this experiment, 8

samples/readings (m = 8) were taken for calibration of each linear accelerometer. A more

general approach to calibrate linear MEMS accelerometers is given in Appendix 6.

The first experiment was performed keeping the base fixed, i.e., NaO = gE1(Figure 2-1). The readings from the encoder are assumed to be the ground truth for

the inclination angle. The plots of comparison of inclination angle, angular velocity and

angular acceleration are shown in Figures 2-16, 2-17, 2-18. The next experiment was

37

0 200 400 600 800 1000 1200 1400−80

−60

−40

−20

0

20

40

60

θ

deg

Time (1 unit = 0.02 sec)

Encoder

VDI

Figure 2-16. Inclination angle measurement when the base is static (no acceleration alongthe surface of contact). Plot of θ from the VDI vs θ from the encoder.

0 200 400 600 800 1000 1200 1400−150

−100

−50

0

50

100

150

θ.

deg/sec

Time (1 unit = 0.02 sec)

Encoder

VDI

Figure 2-17. Plot of angular velocity _θ from the VDI vs _θ from the encoder. Thegyroscope used for the experiment had limit of sensing angular velocitybetween ±75 deg/se as given on the specification sheet.

performed by accelerating the base as shown in Figure 2-11. As mentioned in the earlier

section, this effort is done to analyze the concept of the DEA. The angle φ of the DEA

is calculated by assuming gravitational acceleration as 9.81 m/se 2 and acceleration

obtained from the base accelerometer (Figure 2-14) using Equation 2–43. The measure of

absolute inclination angle β for the case when the base is accelerating is compared with

the encoder angle in Figure 2-19. The experimental results confirm the simulation results

and are very encouraging. The concept of DEA is also experimentally observed.

38

0 200 400 600 800 1000 1200 1400−400

−200

0

200

400

600

θ..

deg/sec2

Time (1 unit = 0.02 sec)

Encoder

VDI

Figure 2-18. Plot of angular acceleration �θ from the VDI vs �θ from the encoder.

0 200 400 600 800 1000−80

−60

−40

−20

0

20

40β = θ+φ

deg

Time (1 unit = 0.02 sec)

EncoderVDI

Figure 2-19. Plot of absolute inclination angle β from the VDI-base accelerometercombination vs angle from the encoder.

39

CHAPTER 3ESTIMATING JOINT PARAMETERS USING THE VDI

Inclination parameters - inclination angle, angular velocity and angular acceleration

are needed for closed-loop feedback control of manipulators, humanoid robots. The

body motion characteristics need to be evaluated during the rehabilitation process of

disabled people [64], [54], [81]. The current method for motion sensing is to use camera

based motion capture systems [49], [17], [55]. This technique is obtrusive and expensive.

It is also difficult to be integrated into a modern medical systems such as a portable

medical device or point-of-care (POC) medication system [81]. Recent advances in

micro-electro-mechanical-systems (MEMS) with high accuracy, high reliability, and

multiple functionalities have provided a powerful tool set for body motion sensing [4], [63],

[74], [40], [26], [79]. A novel way of using the VDI sensors, one on each link, to measure

the inclination parameters is presented. The VDI sensor is contact-less and flexible, thus,

portable and non-obtrusive. Measuring the inclination parameters for the base link (link

in contact with the ground) is also discussed. The desired application for the sensor

are gait analysis, fall evaluation, sports medicine, balance prosthesis, remote patience

surveillance, etc.

3.1 Problem Definition

A mechanism comprised of a series of linkages joined by revolute joints is given. The

linkages are modeled as rigid bodies with known link lengths and the Vestibular Dynamic

Inclinometer(VDI) sensor is located at some convenient distance along the line joining the

two link joints or the point of contact and a joint in case of the base link. The base link

is defined as a link that has a joint at one end of the link and the other end is in contact

with the base (or ground, which is assumed to be stationary) surface as shown in Figure

3-4. Let Oi,j represent the joint joining link i to link j . The line joining joints Oh,i(Obfor base link) and Oi,j(Ob, h for the base link) is referred to as the link vector of the link.

Let the VDI sensor be located at a distance ri along the link vector of link i as shown in

40

VD

Ii

li

ri

Oh,i

Li

eir

eiθ

Oi,j

Pi

VD

I j

L j

ejr

ejθ

r j

l j

θi,j

Oj,k

Figure 3-1. Links i , j joined at point Oi,j having joint angle θi,jFigure 3-1. The joint angle θi,j between links i and j is defined as the angle of rotation

between the respective link coordinate systems. The link length of link i is given by li .Let N represent the inertial reference frame and Li represent the reference frame fixed

on the link i . The link i coordinate system with origin at point Op,i is fixed in the link ireference frame Li , with ei = {eir, eiθ, eiz} orthogonal basis vectors such that eir is alongthe link vector of link i and eiz is into the plane of paper as shown in Figure 3-1. The

joint angle θi,j is the angle between eir and ejr. For the case of the base link, the origin of

the coordinate system lies at the point of contact Ob and the angle between the vertical

(direction of gravitational acceleration) and the link is referred to as the base angle (γb) asshown in Figure 3-4.

It is desired to obtain the base angle (γb), joint angles (θi,js), angular velocities(Nωis)and the angular accelerations(Nαis) of the links with given link lengths(li s) and location of

the VDI sensors(ri s).3.2 Parameter Measurement

For the case of a link i joined to links h and j as shown in Figure 3-2, the inclination

parameters measured by the VDI will be the inclination from the DEA θi , angularvelocity _θi , angular acceleration �θi and magnitude of acceleration ~gi acting at the point of

(virtual) contact Oh,i given the location of the VDI, ri . For a point Q on link i , given the

41

VD

Ii

li

ri

Oh,i

Li

eir

eiθ

θi

Oi,j

gi

~

gj

~φi

Pi

Figure 3-2. The Vestibular Dynamic Inclinometer(VDI) sensor is located on link i oflength li at point Pi which is at distance ri from point Oh,i

displacement vector from point Oh,i to point Q as rOh,i→Q, the acceleration of point Q can

be written as NaQ = NaOh,i + Nαi × rOh,i→Q + Nωi × (Nωi × rOh,i→Q) (3–1)

The resultant acceleration of point Oi,j can be determined in the following mannerNaOi,j = NaPi + (li − ri) (Nαi × eir + Nωi × (Nωi × eir)) (3–2)

The acceleration of point Pi in the Li coordinate system, NaiPi , is the mean of the left and

right accelerometer readings of the VDI and is an observable quantity. The acceleration of

point Oi,j in the Li coordinate system can be written asNaiOi,j = NaiPi + (li − ri) − _θ2i�θi

{eir,eiθ} = ~gj. (3–3)

Let φi be the angle of the resultant acceleration at point Oi,j with eir . It is worthwhile to

indicate that the resultant acceleration of point Oi,j will be parallel to the DEA for the

next link j . For this reason, NaOi,j is referred to as ~gj in Figure 3-2. The angle φi can be

uniquely determined as

φi = atan2((NaiOi,j)θ ,(NaiOi,j)r) (3–4)

42

VDIi

li

ri

~

input output

. ..

θi, φ

i

θi, θ

i, g

i

Figure 3-3. The VDIi sensor function can calculate the inclination parameters andacceleration of joint points as outputs given the inputs of li and ri

VD

Ib

lb

rb

Ob

Lb

ebr

ebθ

γb

Ob,h

g

Figure 3-4. Base link b with one end in contact with ground surface

Where(NaiOi,j)θ ,(NaiOi,j)r denote the tangential and radial components of vector NaiOi,j .

The outputs of the VDIi are summarized in Figure 3-3 with the parameters illustrated

in Figure 3-2. The detailed explanation of how to obtain the output parameters was

discussed earlier.

3.3 Base Link parameters

Given the base link b as in Figure 3-4, the parameters to be estimated are obtained

from the VDIb (Figure 3-3). As shown in the figure, the “ground” surface is assumed

to be at rest (only gravitational acceleration). In this case, the DEA coincides with the

vertical, i.e., direction of gravitational acceleration(g). The inclination parameters (base

angle, angular velocity of the link, and angular acceleration) are obtained in the following

manner

γb = θb (3–5)

43

VD

Ii

li

ri

Oh,i

Li

eir

eiθ

θi

Oi,j

gi

~

φi

Pi

VD

I j

L j

ejr

ejθ

r j

l j

θj

θi,j

~gj

Oj,k

Figure 3-5. Links i , j joined at point Oi,j with joint angle θi,j between themNωb = _θbez (3–6)Nαb = �θbez (3–7)

where ez is the unit vector into the plane of paper.

3.4 Inter-link parameters

For two links i , j joined at point Oi,j as shown in Figure 3-5, it is desired to find the

inclination parameters. The DEA for link j is parallel to the resultant acceleration at

point Oi,j (~gj). The angle between eir and ~gj is given by φi . The value of φi is determined

from VDIi and θj is obtained from VDIj (Figure 3-3). So, the joint angle is estimated as

follows

θi,j = φi + θj (3–8)

The other inclination parameters are estimated as followsNωi = _θiez, Nωj = _θjez (3–9)Nαi = �θiez, Nαj = �θjez (3–10)

44

1

2

O12 θ

12

γ1

O1

g

O2

Figure 3-6. Slider crank mechanism with base angle γ1 and joint angle θ12. Simulationsare performed with link lengths l1 = 0.3m, l2 = 0.6m and sensor locations ofr1 = 0.25m, r2 = 0.5m.

3.5 Example

A slider crank mechanism, shown in Figure 3-6 with link lengths l1 = 0.3m, l2 = 0.6mand sensor locations of r1 = 0.25m, r2 = 0.5m, is simulated in MATLAB R©. The

sensor noise was modeled as mentioned in Section 2.5. The frequency of operation of

the sensors is assumed to be 10 Hz . The location of the linear accelerometers in the

VDI was assumed to be 10 cm apart. The platform is assumed to be stationary. The

simulation results are very encouraging and shown in Figures 3-7 through 3-13. The

estimates for γ1, θ12, Nω1, _θ12, Nα1, �θ12, g and the resultant acceleration of the joints have

standard deviations of 0.31 deg, 0.10 deg, 0.15 deg/sec, 0.15 deg/sec, 11.89 deg/sec2,11.5 deg/sec2, 0.05 m/sec2 and 0.11 m/sec2 respectively. The propagation of error is not

observed in simulations and the estimates do not ‘drift’ over time.

It is observed that the ratio of the distance between the linear accelerometers in

the VDI (d) and the location of the VDI (r) affect the standard deviation errors for

inclination parameters (base angle, joint angle, etc.). For the present simulation, the plots

of the variation of standard deviation of angle, angular velocity, angular acceleration and

resultant acceleration measured are shown in Figures 3-14 through 3-17. As it can be

observed, a change in d/r ratio has an effect on inclination (angular), angular acceleration

and resultant acceleration measurement error (standard deviation). The ideal d/r ratio

may be 1 (which may not be practically possible), but a ratio of 0.4 or above may give a

satisfactory measurement error for common practical applications. As the simulations were

45

0 0.5 1 1.5 2 2.5 3 3.5 4−200

−150

−100

−50

0

50

100

150

200

time

degrees

Base Angle (γ1)

γ

1

observed γ1

Figure 3-7. Plot of estimated γ1 vs true γ1

0 0.5 1 1.5 2 2.5 3 3.5 4−300

−200

−100

0

100

200

300

time

degr

ees/

sec

Base Angular Velocity

Angular VelocityObserved Angular Velocity

Figure 3-8. Plot of estimated Nω1 vs true Nω1done using white noise along the whole frequency spectrum, the results from simulations

are very conservative and are expected to improve when put to practice.

46

0 0.5 1 1.5 2 2.5 3 3.5 4−500

−400

−300

−200

−100

0

100

200

300

400

500

time

degr

ees/

sec2

Base Angular Acceleration

Angular AccelerationObserved Angular Acceleration

Figure 3-9. Plot of estimated Nα1 vs true Nα1

0 0.5 1 1.5 2 2.5 3 3.5 4−150

−100

−50

0

50

100

150

200

250

time

degr

ees

Joint Angle vs observed Joint Angle

Joint Angleobserved Joint Angle

Figure 3-10. Plot of estimated θ12 vs true θ1247

0 0.5 1 1.5 2 2.5 3 3.5 4−400

−300

−200

−100

0

100

200

300

400

time

degr

ees/

sec

Joint Angular Velocity

Joint Angular VelocityObserved Joint Angular Velocity

Figure 3-11. Plot of estimated _θ12 vs true _θ12

0 0.5 1 1.5 2 2.5 3 3.5 4−800

−600

−400

−200

0

200

400

600

time

degr

ees/

sec2

Joint Angular Acceleration vs Observed Angular Acceleration

Joint Angular AccelerationJoint Observed Angular Acceleration

Figure 3-12. Plot of estimated �θ12 vs true �θ1248

0 0.5 1 1.5 2 2.5 3 3.5 42

4

6

8

10

12

14

time

m/s

ec2

Resultant Acceleration of joint

Accelerationobserved Acceleration

Figure 3-13. Plot of estimated joint acceleration NaO12 vs true joint acceleration

49

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2

1.4

Ratio of d/r

Sta

ndar

t dev

iatio

n (d

egre

es)

Angular sensitivity

γ1

θ12

Figure 3-14. Plot of variation of standard deviation of inclination measured with change ind/r ratio.

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10.13

0.135

0.14

0.145

0.15

0.155

0.16

0.165

0.17

0.175

Ratio of d/r

Sta

nd

art

de

via

tio

n (

de

gre

es/

sec)

Angular velocity sensitivity

γ

1

θ12

.

.

Figure 3-15. Plot of variation of standard deviation of angular velocity with change in d/rratio.

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

5

10

15

20

25

30

35

40

45

Ratio of d/r

Sta

nd

art

de

via

tio

n (

de

gre

es/

sec2)

Angular acceleration sensitivity

γ

1

θ12

..

..

Figure 3-16. Plot of variation of standard deviation of angular acceleration with change ind/r ratio. 50

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

Ratio of d/r

Sta

ndar

t dev

iatio

n (m

/sec2 )

Resultant Acceleration sensitivity

at O

1at O

1,2

Figure 3-17. Plot of variation of standard deviation of resultant acceleration with changein d/r ratio.

51

CHAPTER 4THE PLANAR VESTIBULAR DYNAMIC INCLINOMETER (PVDI)

The planar Vestibular Dynamic Inclinometer (pVDI) sensor extends the VDI for five

degrees-of-freedom motion of the robot base. The concept of the Dynamic Equilibrium

Axis (DEA), the axis along which the robot is at equilibrium is preserved from the

VDI design and is discussed. The inclination angle obtained from the pVDI is relative

to the DEA, and thus is more suitable as a control input rather than the inclination

angle relative to the absolute gravity vector (as obtained from other inertial units). The

inclination angle obtained is independent of acceleration of the surface of contact. A

closed form solution for inclination measurement from sensor readings is discussed.

4.1 Problem Definition

A robot is modeled as an inverted pendulum as shown in Figure 4-1. The robot

has five degrees-of-freedom (planar motion of the base and two degrees-of-freedom serial

chain). It is desired to sense the inclination, angular velocity, angular acceleration, and

magnitude of acceleration acting on the body. The rigid body is modeled as a rod with

mass m, center of mass C along the rod at a distance rC from the base of the rod, moment

of inertia IBC at point C and angular damping coefficient Kd . Let point O be at the base

of the rigid body in contact with a base platform which is constrained to move in a plane.

Let N represent the inertial reference frame and B represent the reference frame fixed on

the rigid body. Let g be the gravitational acceleration on the body, a be the acceleration

of point O with respect to the earth (inertial) reference frame and ~g be the resultant of

the previous two mentioned accelerations. Thus~g = g+ a (4–1)

A coordinate system fixed in the inertial reference frame with origin at O, Z-axis

orthogonal to surface of motion, and fixed in reference frame N is defined with {X, Y, Z}as the orthonormal basis. Body coordinate system with origin O is fixed in the body

52

X

Y

Z

e3

e2

e1

g, E3

~

gg

a

~

O

C

P

l

Figure 4-1. Model of robot. Point P denotes the location of the sensor, point C denotesthe center of mass and ~g denotes the resultant acceleration acting on the body.

E1

E2

E3

eb

ec

ea

θ

θ

O

e3

ψ

e2

e1

ψ

Figure 4-2. Definition of Intermediate coordinate system defined by orthogonal basis{ea, eb, e }

reference frame B, with e = {e1, e2, e3} orthogonal basis vectors with e3 along the

vector joining point O to point C . The Dynamic Equilibrium Coordinate system is

defined to be fixed in the inertial reference frame N with origin at point O. The vectorsE = {E1, E2, E3} form a set of orthonormal basis vectors such that E3 is parallel to vector~g. Point P is a point on the body at a distance l from point O as shown in Figure 4-1.

4.2 Intermediate Coordinate System and Rotation Matricies

The body possesses two angular degrees-of-freedom, i.e., a minimum of two rotations

are required to align the body coordinate system to the Dynamic Equilibrium Coordinate

53

system. An intermediate coordinate system is defined with origin at point O and

orthogonal basis I = {ea, eb, e } obtained by rotation of the Dynamic Equilibrium

coordinate system with angle an θ along the E1 axis as shown in Figure 4-2. The

intermediate coordinate system aligns with the body coordinate system when rotated

with an angle ψ about axis eb. The angular velocity of the body reference frame B in the

inertial reference frame N, NωB may be written asNωB = _θE1 + _ψe2 (4–2)

It is worthwhile to mention that the unit vectors E1, e2 are the rotation axes of the Hooke

joint at point O. The angular acceleration of reference frame B in reference frame N, NαB,can be calculated using the transport theorem [59]NαB = �θE1 + �ψe2 + (NωB × ( _ψe2)) (4–3)

Rotation matrices [18] to transform from basis I to basis E and from basis e to basis I aredefined by IER and eIR respectively asIER = R1(θ) =

1 0 00 θ sθ0 −sθ θ

(4–4)

eIR = R2(ψ) =

ψ 0 −sψ0 1 0sψ 0 ψ

(4–5)eER = eIR IER (4–6)

54

where sj = sin(j), j = os(j), j = θ,φ. Let NωBe and NαBe be the matrix representation of

the angular velocity and acceleration represented in the basis e. ThusNωBe =

_θ ψ_ψ_θsψ

(4–7)

NαBe =

�θ ψ − _θsψ _ψ�ψ�θsψ + _θ ψ _ψ

. (4–8)

As the simple rotations required to reach the final coordinate system are rotation of θ

about axis e1 (R1(θ)) and rotation of ψ about axis eb (R2(ψ)), for future references,

the ψ, θ combination is denoted as 1-2 Euler angles. Assuming that the robot has

motors/angular actuators that allow the movement of the robot along directions E1and e2/eb (equivalent hooke-joint), it is desired to find angles ψ and θ as given in Figure

4-2 that realign the robot along the DEA.

4.3 Sensor Design

The sensor analogy of the human vestibular system has been done as follows -

dual-axis accelerometers are assumed to be analogous to the otoliths organs and the

semicircular canals are visualized as one-dimensional gyroscope. As the robot/human

experiences planar motion, it is natural to draw a deeper analogy with the human

vestibular system. The human vestibular system consists of three semicircular canals

that are approximately orthogonal to each other. This motivates the use of one 3-axis

gyroscope in the design of the new sensor. As the linear acceleration is sensed by

stimulation of hair cells due to the movement of viscous fluid in the saccule and utricle,

one otoliths organ is assumed to be analogous to two linear accelerometers placed at some

distance.

55

The planar Vestibular Dynamic Inclinometer (pVDI) is designed by strategically and

symmetrically placing four 2-axis accelerometers and one 3-axis gyroscope as shown in

Figure 4-3. The hollow arrows give the directions of the dual-axis linear accelerometers.

Unlike the VDI in [72], the directions of the dual-axis linear accelerometers are different.

The location of the sensor is at point P which is at distance l from point O in the e3direction. rP = rO + l e3. (4–9)

The gyroscope is placed at point P. Linear accelerometers L1 and R1 are placed

symmetrically at a distance d1/2 along the e1 direction about point P and measure

the acceleration along e3 and e2. Similarly, linear accelerometers L2 and R2 are placed

symmetrically at distance d2/2 along e2 direction about point P and measure the

acceleration along e3 and e1 as shown in Figure 4-3. ThusrOL1 = l e3 − d12 e1 (4–10)rOR1 = l e3 + d12 e1 (4–11)rOL2 = l e3 − d22 e2 (4–12)rOR2 = l e3 + d22 e2. (4–13)

4.4 Dynamic Equilibrium Axis for planar motion of the base

For point C on the body, performing a kinematic analysis to obtain the linear

acceleration in reference frame N (NaC ) yieldsNaC = NaO + NαB × rOC + NωB × (NωB × rOC) (4–14)

where rOC denotes the vector from point O to point C andNaO = ~g = ~ge3 (4–15)

56

e1

e2

e3

R2

L2

d1/2

d1/2

d2/2d

2/2

GP

R1

L1

Figure 4-3. Sensor is made of accelerometers L1,R1, L2,R2 and gyroscope G . Hollowarrows indicate the orientation of the dual-axis linear accelerometers at therespective locations

Let the reacting forces acting on the body at the point of contact O be FR. Application of

Euler’s first and second law about center of mass C givesm · NaC = FR (4–16)ICB · NαB = −KdNωB + rCO × FR (4–17)

At equilibrium �θ∗ = _θ∗ = 0 and �ψ∗ = _ψ∗ = 0. Therefore, from Equations 4–2, 4–3,

4–14, 4–15, 4–16 and 4–17, the equilibrium position is θ∗ = ψ∗ = 0, i.e., rCO is parallel

to ~g. The axis parallel to E3 is called the Dynamic Equilibrium Axis (DEA) for planar

motion of the base. When the body is aligned along this axis, it is in equilibrium. The

Dynamic Equilibrium Axis (DEA) is parallel to the resulting acceleration acting on the

body ~g, thus, is time-varying (more precisely, acceleration varying). This is similar to

the DEA that has been discussed earlier. The objective for robot balancing applications

is to bring the robot to equilibrium, i.e., to align the robot along the DEA. The robot

possesses five degrees-of-freedom - two translational (one less due to surface contact) and

three rotational. The analysis above shows that the equilibrium position for the robot

is not a point or a surface, but an axis called the Dynamic Equilibrium Axis (DEA).

57

e3

e2

e1

O

C

P

l

E3

E1

E2

g~

Figure 4-4. E3 is defined to be parallel to resultant acceleration vector ~g. Axes E1 and e2are parallel to the two hooke-joint axes.

Aligning the robot along the axis requires one to move the robot about an equivalent

hooke-joint (two rotational degrees-of-freedom). The orientation of the body about the

DEA is irrelevant for robot equilibrium, thus, requiring only two independent parameters

(equivalent hooke-joint) to align the robot along the DEA. It should also be observed that

when the robot is not in contact with the ground and experiences free fall, the concept

of the DEA ceases to exist as the acceleration experienced by the linear accelerometer at

point O is zero, i.e., ~g = 0. Theoretically, it reinforces the concept of the DEA as the

concept of ‘equilibrium position’ ceases to exist in zero gravity.

4.5 Kinematic Analysis and Mathematical Manipulations

Acceleration of any point Q on the body can be written asNaQ = NaO + (NαB × rOQ) + NωB × (NωB × rOQ) (4–18)

where Q may be {L1,R1, L2,R2}. Gyroscope and accelerometers sense the angular velocity

and linear accelerations in the basis e. The following quantities are defined in terms of the

58

measured accelerometer and gyroscope data as

ζ1 = NaR1 − NaL1d1 (4–19)

ζ2 = NaR2 − NaL2d2 (4–20)

ζ3 = NaR2 + NaL22l = NaR1 + NaL12l (4–21)

ζ4 = NωBe . (4–22)

Simplifying the quantities using Equations 4–10-4–18 yields

ζ1 = NαB × e1 + NωB × (NωB × e1) (4–23)

ζ2 = NαB × e2 + NωB × (NωB × e2) (4–24)

ζ3 = NaOl + NαB × e3 + NωB × (NωB × e3) . (4–25)

4.6 Inclination Measurement - Closed form solution

Representing equations 4–22 - 4–25 in the basis e, i.e., the basis in which the sensor

readings are actually observed, is accomplished by using Equations 4–7, 4–8 and 4–15 to

obtain

ζ1 =

�������− _ψ2 − _θ2s2ψ�θsψ + 2 _θ ψ _ψ− �ψ + _θ2 ψsψ

(4–26)

ζ2 =

−�θsψ���− _θ2�θ ψ

(4–27)

ζ3 =

�ψ − (~gsψ θ)/l + _θ2 ψsψ(~gsθ)/l − �θ ψ + 2 _θsψ _ψ− _ψ2 + (~g ψ θ)/l − _θ2 2ψ

(4–28)

59

ζ4 =

_θ ψ_ψ_θsψ

. (4–29)

The components of the vectors ζi , i = 1, 2, 3, 4, will be referred to as ζij , j = 1, 2, 3.The values that are not observed due to sensor placement are ζ11, ζ22 (striked out in the

equations). The mean of readings of linear accelerometers {R1, L1} and {R2, L2} provide{ζ32, ζ33} and {ζ31, ζ33} respectively, thus, all three components of ζ3 are observed.

The terms ζij are manipulated to give seven equations in seven unknowns (θ, _θ, �θ,ψ, _ψ, �ψ, ~g)�ψ = −ζ13 + ζ41ζ43 (4–30)_ψ = ζ42 (4–31)�θ ψ = ζ23 (4–32)~gsψ θ = l(−ζ31 − ζ13 + 2ζ41ζ43) (4–33)~gsθ = l(ζ32 + ζ23 − 2ζ42ζ43) (4–34)_θ ψ = ζ41 (4–35)_θsψ = ζ43 (4–36)

Other equations that fall out are�θsψ = −ζ21 = −(ζ12 + 2ζ41ζ42) (4–37)~g ψ θ = l(ζ33 + ζ242 + ζ241) (4–38)

The function getVal (α, β1, β2) solves the pair of equations a sin(α) = β1, a os(α) = β2 fora, given α ∈ (−π/2, π/2)getVal (α, β1, β2) = sign( α) · sign(β2)√β21 + β22 (4–39)

60

Algorithm 1 Measurement of inclination parameters from sensor data

Require: {ζ12, ζ13}, {ζ21, ζ23}, ζ3, ζ4Let η1, η2, η3 are defined as follows

η1 = l(−ζ31 − ζ13 + 2ζ41ζ43)η2 = l(ζ33 + ζ242 + ζ241)η3 = l(ζ32 + ζ23 − 2ζ42ζ43)

Require: |η21 + η22| > ǫ1 and |η21 + η22 + η33| > ǫ2�ψ ← (−ζ13 + ζ41ζ43)_ψ ← ζ42ψ ← tan−1 (η1

η2)ψ ∈ (−π/2, π/2)_θ← getVal (ψ, ζ43, ζ41)�θ← getVal (ψ, (( ζ12−ζ212 )

− ζ41ζ43) , ζ23)Let η4 = getVal (ψ, η1, η2)θ← atan2(η3, η4)

θ ∈ (−π, π℄~g ← getVal (θ, η3, η4)4.7 Sensor Simulation

Simulations were performed in MATLAB R©. The sensor noise was modeled as

mentioned in Section 2.5. The frequency of operation of the sensors is assumed to be10 Hz . It should be indicated that when ~g is zero, the concept of the DEA ceases to exist.

In such cases, the measurement for inclination parameters is invalid. The case θ = π/2is not possible as eb/e2 aligns with e3 after the first rotation and no possible rotation

along e2 can align e with e3. So, the possibility of estimation of inclination parameters

exists only when ~g θ 6= 0. The simulation results are very good and encouraging. One

set of simulation results are shown in Figures 4-5A, 4-5B, 4-6A, 4-6B, 4-7A, 4-7B and 4-8.

The estimates for θ,ψ, _θ, _ψ, �θ, �ψ and ~g have a standard deviations of 0.46 deg, 0.45 deg,0.15 deg/sec, 0.15 rad/sec, 4.5 rad/se 2, 4.6 rad/se 2 and 0.05 m/se 2 respectively. The

estimates do not ‘drift’ over time as it does not involve integration of the quantities.

4.8 Sensor Experiment

The experiment was set with four dual-axis linear MEMS accelerometers (ADXL335)

symmetrically placed about the center-line of the inverted pendulum as shown in Figure

61

0 1 2 3 4 5 6−40

−30

−20

−10

0

10

20

30

40

Comparison of θ

Time (sec)

θ (

deg

)

True θ

Observed θ

(A) Plot of estimated θ vs true θ

0 1 2 3 4 5 6−50

−40

−30

−20

−10

0

10

20

30

40

50

Comparison of ψ

Time (sec)

ψ (

deg

)

True ψ

Observed ψ

(B) Plot of estimated ψ vs true ψ

Figure 4-5. Plots of angles θ,ψ

0 1 2 3 4 5 6−40

−30

−20

−10

0

10

20

30

40

Comparison of θ

Time (sec)

θ (

deg

/sec

)

True θ

Observed θ

.

.

.

.

(A) Plot of estimated _θ vs true _θ 0 1 2 3 4 5 6−50

−40

−30

−20

−10

0

10

20

30

40

50

Comparison of

Time (sec)

ψ (

deg

/sec

)

True ψ

Observed ψ

ψ.

.

.

.

(B) Plot of estimated _ψ vs true _ψFigure 4-6. Plots of angular velocities _θ, _ψ

0 1 2 3 4 5 6−50

−40

−30

−20

−10

0

10

20

30

40

50

Comparison of θ

Time (sec)

θ (

deg

/sec

2)

True θ

Observed θ

..

..

..

..

(A) Plot of estimated �θ vs true �θ 0 1 2 3 4 5 6−60

−40

−20

0

20

40

60

Comparison of ψ

Time (sec)

ψ (

deg

/sec

2)

True ψ

Observed ψ

..

..

..

..

(B) Plot of estimated �ψ vs true �ψFigure 4-7. Plots of angular accelerations �θ, �ψ

62

0 1 2 3 4 5 66

7

8

9

10

11

12

Estimation of g

Time (sec)

g (

m/s

2)

True g

Estimated g

~

~~

~

Figure 4-8. Plot of estimated ~g vs true ~g4-9. The single-axis MEMS gyroscope (LPY503AL) was combined with a dual-axis

gyroscope (LPY503AL) to make a triaxial gyroscope and strapped onto the inverted

pendulum as shown in Figure 4-9. Interfacing of the analog voltage signal was done using

NI-DAQmx and LabVIEW R©. Magnetic encoders (US Digital MA3-A10-125-N) were fixed

at the joints. The measurement of the encoder angle was assumed to be the ground truth.

The accelerometers were autocalibrated and the mis-alignment of the accelerometers to

the ‘theoretical’ radial and tangential directions in the body coordinate system e was done

using the technique given in Appendix 6.

The experiment was performed keeping the base fixed, i.e., NaO = gE1 (Figure

4-1). The readings from the encoder are assumed to be ground truth for inclination

angles (θ,ψ). The plots of comparison of inclination angle, angular velocity and angular

acceleration are shown in Figures 4-10A, 4-10B, 4-10A, 4-10B, 4-10, 4-11A. The lag in the

encoder angular velocity and angular acceleration signal appears due to differentiation of

the encoder signal and then smoothing it. The experimental results confirm the simulation

results and are very encouraging. The concept of the DEA is also experimentally

observed.

63

MEMS Linear

Accelerometers

MEMS

Gyrocope

50 cm

22 cm

Magnetic

Encoders

22 cm

Figure 4-9. Experimental setup of the planar Vestibular Dynamic Inclinometer(pVDI).The four symmetrically placed linear MEMS accelerometers are marked inboxes. The MEMS gyroscope is marked by circle. The magnetic encoders areplaced to measure the Euler angles.

0 100 200 300 400 500 600 700−50

−40

−30

−20

−10

0

10

20θ

deg

Time (1 unit = 0.02 sec)

EncoderpVDI

(A) Comparison of θ from the pVDI vs θ from theencoder.

0 100 200 300 400 500 600 700−40

−30

−20

−10

0

10

20

30

40ψ

deg

Time (1 unit = 0.02 sec)

EncoderpVDI

(B) Comparison of ψ from the pVDI vs ψ from theencoder.

64

0 100 200 300 400 500 600 700−60

−40

−20

0

20

40

60

80

θ.

deg/sec

Time (1 unit = 0.02 sec)

Encoder

pVDI

(A) Plot of _θ from the pVDI vs _θ from the encoder.

0 100 200 300 400 500 600 700−80

−60

−40

−20

0

20

40

60

80

ψ.

deg/sec

Time (1 unit = 0.02 sec)

Encoder

pVDI

(B) Plot of _ψ from the pVDI vs _ψ from the encoder.

0 100 200 300 400 500 600 700−300

−200

−100

0

100

200

300

400

θ..

deg/sec2

Time (1 unit = 0.02 sec)

Encoder

pVDI

Figure 4-10. Plot of �θ from the pVDI vs �θ from the encoder.

0 100 200 300 400 500 600 700−400

−300

−200

−100

0

100

200

300

400

ψ..

deg/sec2

Time (1 unit = 0.02 sec)

Encoder

pVDI

(A) Plot of �ψ from the pVDI vs �ψ from the encoder.

65

CHAPTER 5ESTIMATING JOINT PARAMETERS USING PVDI

The use of VDI for measuring link parameters of two links joined by revolute joints

(e.g., knee joint) has been discussed in Chapter 3. The application of pVDI to measure

link parameters of two links joined by universal/Hooke joint is presented. The core

concept of obtaining resultant acceleration at the joint to obtain the joint angles is

extended. The approach remains similar to that presented in Chapter 3.

5.1 Problem Definition

The problem definition proceeds in a similar fashion as presented in Chapter 3.1. A

mechanism comprised of a series of linkages joined by universal/Hooke joints is given. The

linkages are modeled as rigid bodies with known link lengths and the planar Vestibular

Dynamic Inclinometer(pVDI) sensor is located at some convenient distance along the line

joining the two link joints or the point of contact and a joint in case of the base link. The

base link is defined as a link that has a joint at one end of the link and the other end is

in contact with the base (or ground, which is assumed to be stationary) surface as shown

in Figure 5-4. Let Oi,j represent the joint joining link i to link j . The line joining jointsOh,i(Ob for base link) and Oi,j(Ob, h for the base link) is referred to as the link vector of

the link. Let the VDI sensor be located at a distance ri along the link vector of link i asshown in Figure 3-1. The joint angles θi,j , ψi,j between links i and j are defined as 1-2

Euler rotation angles (Figure 4-2) between the respective link coordinate systems. The

link length of link i is given by li .Let N represent the inertial reference frame and Li represent the reference frame fixed

on the link i . The link i coordinate system with origin at point Op,i is fixed in the link ireference frame Li , with ei = {ei1, ei2, ei3} orthogonal basis vectors such that ei3 is along

the link vector of link i and ei2 is along the second rotation axis of the hook joint (T).

The joint angles θi,j , ψi,j are the Euler 1-2 angles between ei3 and ej3. For the case of the

base link, the origin of the coordinate system lies at the point of contact Ob and the Euler

66

pVD

Ii

li

ri

Oh,i

Li

ei3

ei1

ei2

Oi,j

Pi

pVD

I j

L j

ej3

ej2

ej1

r j

l j

{θi,j,ψ

i,j}

Oj,k

T

Li

ei1

ej1

ej2

ej3

ei2

ei3

Lj

Oij

Figure 5-1. Links i , j joined at point Oi,j having joint angles θi,j ,ψi,j between them.

1-2 angles between the vertical (direction of gravitational acceleration) and the link are

referred to as the base angles (γb, δb) as shown in Figure 5-4.

It is desired to obtain the base angles (γb, δb), joint angles (θi,js, ψi,js), angularvelocities(Nωis) and the angular accelerations(Nαis) of the links with given link lengths(li s)and location of the VDI sensors(ri s).

5.2 Parameter Measurement

For the case of a link i joined to links h and j as shown in Figure 5-2, the inclination

parameters measured by the pVDI will be the Euler 1-2 inclination angles from the DEA

θi ,ψi , angular velocities _θi , _ψi , angular acceleration �θi , �ψi and magnitude of acceleration~gi acting at the point of (virtual) contact Oh,i given the location of the pVDI, ri . For a

point Q on link i , given the displacement vector from point Oh,i to point Q as rOh,i→Q, theacceleration of point Q can be written asNaQ = NaOh,i + Nαi × rOh,i→Q + Nωi × (Nωi × rOh,i→Q) (5–1)

The resultant acceleration of point Oi,j can be determined in the following mannerNaOi,j = NaPi + (li − ri) (Nαi × ei3 + Nωi × (Nωi × ei3)) (5–2)

67

pVD

Ii

li

ri

Oh,i

Li

ei3

ei1

ei2

{θi,ψ

i}

Oi,j

gi

~

gj

~{φ

i,δ

i}

Pi

Figure 5-2. The planar Vestibular Dynamic Inclinometer(pVDI) sensor is located on link iof length li at point Pi which is at distance ri from point Oh,i

The acceleration of point Pi in the Li coordinate system, NaiPi , is the mean of the left and

right accelerometer readings of the VDI and is an observable quantity. The acceleration of

point Oi,j in the Li coordinate system can be written asNaiOi,j = NaiPi + (li − ri)

�ψ − (~gsψ θ)/l + _θ2 ψsψ(~gsθ)/l − �θ ψ + 2 _θsψ _ψ− _ψ2 + (~g ψ θ)/l − _θ2 2ψ

ei = ~gj. (5–3)

The calculation of ~gj can be done as follow~gj = riζ3 + (li − ri)

2ζ41ζ43 − ζ13−ζ23 + 2ζ42ζ43−ζ241 − ζ242

ei (5–4)

Let (φi , δi) be the Euler 1-2 angles of the resultant acceleration at point Oi,j from ei3. It

is worthwhile to indicate that the resultant acceleration of point Oi,j will be parallel to the

DEA for the next link j . For this reason, NaOi,j is referred to as ~gj in Figure 5-2. The angle

φi can be uniquely determined as

δ = sin−1( ~gj1||~gj ||) , φ = atan2(− δ ~gj2

||~gj || , δ ~gj3||~gj ||) (5–5)

68

pVDIi

li

ri

~

input output

. .{θ

i,ψ

i},{φ

i,δ

i}

{θi,ψ

i}

.. ..{θ

i,ψ

i}

,gi

Figure 5-3. The pVDIi sensor function can calculate the inclination parameters andacceleration of joint points as outputs given the inputs of li and ri

pVD

Ib

lb

rb

Ob

Lb

eb3

eb1

{γb,ϕ

b}

Ob,h

g

Figure 5-4. Base link b with one end in contact with ground surface

where ~gjk , k = 1, 2, 3 are the kth components of ~gj. The outputs of the pVDIi aresummarized in Figure 5-3 with the parameters illustrated in Figure 5-2. Detail to obtain

the output parameters have been discussed earlier.

5.3 Base Link parameters

Given the base link b as in Figure 5-4, the parameters to be estimated are obtained

from the pVDIb (Figure 5-3). As shown in the figure, the “ground” surface is assumed

to be at rest (only gravitational acceleration). In this case, the DEA coincides with the

vertical, i.e., direction of gravitational acceleration(g). The inclination parameters (base

angles, angular velocity of the link, and angular acceleration) are obtained in the following

69

pVD

Ii

li

ri

Oh,i

Li

ei3

ei1

ei2

{θi,ψ

i}

Oi,j

gi

~

{φi,δ

i}

Pi

pVD

I j

L j

ej3

ej1

ej2

r j

l j

{θj,ψ

j}

{θi,j,ψ

i,j}

~gj

Oj,k

T

T

Figure 5-5. Links i , j joined at point Oi,j with Euler 1-2 joint angles θi,j , ψi,j between them

manner

γb = θb, ϕb = δb (5–6)Nωb =

_θb ψb_ψb_θbsψb

eb (5–7)

Nαb =

�θb ψb − _θbsψb _ψb�ψb�θbsψb + _θb ψb _ψb

eb (5–8)

5.4 Inter-link parameters

For two links i , j joined at point Oi,j as shown in Figure 5-5, it is desired to find the

inclination parameters. The DEA for link j is parallel to the resultant acceleration at

point Oi,j (~gj). The 1-2 Euler angles between ei3 and ~gj are given by φi , δi . The values

of φi , δi are determined from VDIi and θj ,ψj are obtained from pVDIj (Figure 5-3). LeteiejR represent the rotation matrix [18] to transform from basis ei to basis ej. So, the joint

70

angles are estimated as followseiejR = R1(φi)R2(δi)R1(θj)R2(ψj) =

ψij 0 sψijsθi,jsψi,j θi,j −sθi,j ψi,j− θi,jsψi,j sθi,j θi,j ψi,j

(5–9)

θi,j = atan2(eiejR32, eiejR22) (5–10)

ψi,j = atan2(eiejR13, eiejR11) (5–11)

where Ri(ρ) is the rotation matrix representing rotation about ith Euler axis by angle ρ.

The other inclination parameters are estimated as followsNωi =

_θi ψi_ψi_θisψi

ei (5–12)

Nαi =

�θi ψi − _θisψi _ψi�ψi�θisψi + _θi ψi _ψi

ei (5–13)

71

CHAPTER 6CONCLUSION AND FUTURE WORK

The Vestibular Dynamic Inclinometer (VDI) and planar Vestibular Dynamic

Inclinometer (pVDI) are human vestibular system motivated inclination measurement

sensors. They measure the inclination parameters - angular inclination, angular velocity,

angular acceleration and magnitude of acceleration of the surface of contact. The VDI

and pVDI measure the inclination parameters for four degrees-of-freedom robot and five

degrees-of-freedom robots respectively. The inclination measurements are independent of

the acceleration of the surface of contact (gravity, etc.), any drift/integration errors and

valid for large inclination angles.

The design of the VDI sensor consists of two symmetrically placed dual-axis MEMS

linear accelerometers and one single axis MEMS gyroscope. While, for the pVDI sensor,

the design consists of strategically and four symmetrically placed dual-axis MEMS linear

accelerometers and one tri-axial MEMS gyroscope. The orientation of the symmetrically

placed dual-axis linear accelerometers for the pVDI is different from that of the analogous

accelerometers in the case of the VDI.

The Dynamic Equilibrium Axis (DEA) is the axis along which the robot is at

equilibrium. The DEA, which is parallel to the direction of the resultant acceleration of

the platform/surface of contact (gravity, etc), acts as the reference for measurement. The

DEA ceases to exist when the resultant acceleration of the contact platform/surface is

zero, i.e., zero gravity (e.g., free-fall). The DEA helps to explain the leaning when humans

accelerate and bending back when they decelerate.

The inclination measurements are valid for environments with varying gravity (space

applications) and accelerating platforms (running, walking motion of robots, etc.). The

sensor outputs are ideal control inputs for balancing of robots as the goal is to bring the

robots to the equilibrium position (i.e., align it along the DEA). These makes the VDI

and pVDI ideal for balancing of robots, humanoids, etc. Due to economical advantages

72

of MEMS sensors, the applications of VDI and pVDI extend to joint angle measurement,

human movement studies e.g., gait analysis, arm movement about elbow, etc. The joint

angle measurement of biped robots is also a potential application.

As the results from the simulation are very conservative, the future work requires to

attempt to shrink the total size of the sensors and develop calibration techniques to deal

with errors in distance measurements. Algorithm to correct sensor distance misalignment

(asymmetry) needs to be researched. It is also desired to apply the sensors to measure

inclination parameters of spatial manipulators (all revolute joints).

73

APPENDIX: AUTOCALIBRATION OF MEMS ACCELEROMETERS

An autocalibration procedure based on the assumption that a static inertial sensor

only experiences force due to gravity [23]. The resultant of the acceleration vector

measured by the sensor must equal g = 9.81 m/s2. This method requires measuring

output signals (V) of the MEMS accelerometer in nine different directions. As the adopted

accelerometers are designed to provide ratio-metric output, let V T = [vx , vy , vz ℄ representthe ratio of output voltage to power supply voltage VCC (i.e. vi = Vi/VCC i = x , y , z).

A.1 Problem Statement

Let the acceleration vector be defined as AT = [ax , ay , az ℄ represented in the sensor

coordinate system. The inertial coordinate system is fixed in inertial reference frame Nwith orthogonal basis {X ,Y ,Z} and origin at point O. The the sensor coordinate system

is fixed in the sensor reference frame with orthonormal basis {e1, e2, e3} and origin at

point O as shown in Figure A-1. The sensor is modeled as followsA = S(V −O) (A–1)

where the sensitivity S and bias O areS =

Sxx Sxy SxzSyx Syy SyzSzx Szy Szz

, O =

OxOyOz

(A–2)

After imposing the symmetry constraint (Sxy = Syx , Sxz = Szx , Szy = Syz , the nine

unknown parameters are written as X9×1 = [x1, x2, · · · , x9℄ = [Sxx ,Syy ,Szz ,Sxy ,Syz ,Szx ,Ox ,Oy ,Oz ℄.It is desired to find the unknown parameters X and misalignment angles ϕ, ρ (Figure

A-1).

74

ϕ

ρ

e1

Y

e3

Z

X

e2

g (gravity)

O

Figure A-1. The sensor coordinate system has constant misalignment from the inertialcoordinate system that can be quantized by ρ,ϕ.

A.2 Autocalibration

The autocalibration procedure uses the concept that in static orientations, the

resultant acceleration experienced by the sensor is only due to gravity i.e. g = 9.81 m/se 2.||A|| = √a2x + a2y + a2z = g (A–3)

Let Ai denote corresponding accelerometer reading when the sensor is in the ithconfiguration. For N configurations, defining the least squares error EE(X ) = 1N N

∑i=1 {||Ai ||2 − g2} = N∑i=1 {||S(Vi −O)||2 − g2} (A–4)

Autocalibration of the sensor will be the solution to the mathematical problemargminX E(X ) (A–5)

75

where E(X ) is a non-linear function in unknown parameters. Starting from initial guess of

the sensor parameters as given by the manufacturers, the solution is iteratively updated asX k+1 = X k − αH−1(X k) · J(X k) (A–6)

for kth iteration, where H(X ), J(X ) are the Hessian matrix and Jacobian vector for the

error E , respectively.J(X ) = [

∂E∂x1 , · · · , ∂E∂x9] , H(X ) = {hij = ∂2E

∂xi∂xj} (A–7)

α is step parameter which is less than 1 and computed on each iteration using line search

procedure. The termination procedure is chosen asmaxm=1,··· ,9{ xk+1m − xkm(xk+1m − xkm)/2} < ǫ (A–8)

where ǫ is some threshold.

A.3 Misalignment Computation

The misalignment angles can be calculated when the sensor is kept flat on the ground.

Then the acceleration experienced by the sensor will be

axayaz

{e1,e2,3} = gEz = g

−sρsϕsρ ρ ϕ

{e1,e2,3} (A–9)

So,

ρ = sin−1{ayg } , ϕ = atan2 (−sρax , ρaz) (A–10)

76

REFERENCES

[1] Algrain, M.C. and Saniie, J. “Estimation of 3D angular motion using gyroscopes andlinear accelerometers.” IEEE Transactions on Aerospace and Electronic Systems 27(1991).6: 910–920.

[2] Bachmann, E. R. Inertial And Magnetic Tracking Of Limb Segment Orientation ForInserting Humans Into Synthetic Environments. Storming Media, 2000.

[3] Baerveldt, A.J. and Klang, R. “A low-cost and low-weight attitude estimationsystem for an autonomous helicopter.” IEEE International Conference on IntelligentEngineering Systems. 1997, 391–395.

[4] Barbour, N. and Schmidt, G. “Inertial sensor technology trends.” IEEE SensorsJournal 1 (2001).4: 332–339.

[5] Barshan, B. and Durrant-Whyte, H.F. “Inertial navigation systems for mobilerobots.” IEEE Transactions on Robotics and Automation 11 (1995).3: 328–342.

[6] Baselli, G., Legnani, G., Franco, P., Brognoli, F., Marras, A., Quaranta, F., andZappa, B. “Assessment of inertial and gravitational inputs to the vestibular system.”Journal of Biomechanics 34 (2001).6: 821–826.

[7] Baxter, L. K. Capacitive Sensors: Design and Applications (IEEE Press Series onElectronics Technology). Wiley-IEEE Press, 1996.

[8] Bernmark, E. and Wiktorin, C. “A triaxial accelerometer for measuring armmovements.” Applied Ergonomics 33 (2002).6: 541–547.

[9] Bernstein, J. “An overview of MEMS inertial sensing technology.” Sensors - TheJournal of Applied Sensing Technology 20 (2003).2: 14–21.

[10] Billat, S., Glosch, H., Kunze, M., Hedrich, F., Frech, J., Auber, J., Lang, W.,Sandmaier, H., and Wimmer, W. “Convection-based micromachined inclinometerusing SOI technology.” The 14th IEEE International Conference on Micro ElectroMechanical Systems, 2001. IEEE, 2001, 159–161.

[11] Bortz, J.E. “A new mathematical formulation for strapdown inertial navigation.”IEEE Transactions on Aerospace and Electronic Systems (1971).1: 61–66.

[12] Bouten, C.V.C., Koekkoek, K.T.M., Verduin, M., Kodde, R., and Janssen, J.D.“A triaxial accelerometer and portable data processing unit for the assessment ofdaily physical activity.” IEEE Transactions on Biomedical Engineering 44 (1997).3:136–147.

[13] Brandt, T. Vertigo: Its Multisensory Syndromes. Springer Verlag, 1999.

[14] Camis, M. and Creed, R.S. “The physiology of the vestibular apparatus.” TheAmerican Journal of the Medical Sciences 180 (1930).6: 849.

77

[15] Chen, J.H., Lee, S.C., and DeBra, D.B. “Gyroscope free strapdown inertialmeasurement unit by six linear accelerometers.” Journal of Guidance, Control,and Dynamics 17 (1994).2: 286–290.

[16] Cheng, P. and Oelmann, B. “Joint-angle measurement using accelerometers andgyroscopes - A survey.” IEEE Transactions on Instrumentation and Measurement 59(2010).2: 404–414.

[17] Corazza, S., Mundermann, L., Chaudhari, A.M., Demattio, T., Cobelli, C., andAndriacchi, T.P. “A markerless motion capture system to study musculoskeletalbiomechanics: Visual hull and simulated annealing approach.” Annals of BiomedicalEngineering 34 (2006).6: 1019–1029.

[18] Crane, C.D. and Duffy, J. Kinematic Analysis of Robot Manipulators. CambridgeUniversity Press, 1998.

[19] Damiano, E.R. “A poroelastic continuum model of the cupula partition and theresponse dynamics of the vestibular semicircular canal.” Journal of BiomechanicalEngineering 121 (1999): 449–461.

[20] Damiano, E.R. and Rabbitt, R.D. “A singular perturbation model of fluid dynamicsin the vestibular semicircular canal and ampulla.” Journal of Fluid Mechanics 307(1996).1: 333–372.

[21] Dejnabadi, H., Jolles, B.M., and Aminian, K. “A new approach to accuratemeasurement of uniaxial joint angles based on a combination of accelerometers andgyroscopes.” IEEE Transactions on Biomedical Engineering 52 (2005).8: 1478–1484.

[22] Foxlin, E. “Inertial head-tracker sensor fusion by a complimentary separate-biaskalman filter.” Proceedings of Virtual Reality Annual International Symposium. IEEE,1996, 185–194.

[23] Frosio, I., Pedersini, F., and Borghese, N.A. “Autocalibration of MEMSaccelerometers.” IEEE Transactions on Instrumentation and Measurement 58(2009).6: 2034–2041.

[24] Ghassemi, F., Tafazoli, S., Lawrence, P.D., and Hashtrudi-Zaad, K. “Anaccelerometer-based joint angle sensor for heavy-duty manipulators.” IEEE In-ternational Conference on Robotics and Automation. vol. 2. IEEE, 2002, 1771–1776.

[25] ———. “Design and calibration of an integration-free accelerometer-based joint-anglesensor.” IEEE Transactions on Instrumentation and Measurement 57 (2007).1:150–159.

[26] Godfrey, A., Conway, R., Meagher, D., and OLaighin, G. “Direct measurement ofhuman movement by accelerometry.” Medical Engineering and Physics 30 (2008).10:1364–1386.

78

[27] Horak, F.B. and Macpherson, J.M. “Postural orientation and equilibrium.” Compre-hensive Physiology (1996): 255–292.

[28] Ignagni, M.B. “Optimal strapdown attitude integration algorithms.” Journal ofGuidance and Control 13 (1990).2.

[29] Jiang, Y.F. and Lin, Y.P. “Improved strapdown coning algorithms.” IEEE Transac-tions on Aerospace and Electronic Systems 28 (1992).2: 484–490.

[30] Kalman, R.E. et al. “A new approach to linear filtering and prediction problems.”Journal of Basic Engineering 82 (1960).1: 35–45.

[31] Kemp, B., Janssen, A.J.M.W., and van der Kamp, B. “Body position can bemonitored in 3D using miniature accelerometers and earth-magnetic field sensors.”Electroencephalography and Clinical Neurophysiology 109 (1998).6: 484–488.

[32] Khalil, H.K. Nonlinear systems. Prentice Hall, 2001, 3 ed.

[33] King, A.D. “Inertial navigation-forty years of evolution.” General Elecric CompanyReview 13 (1998).3: 140–149.

[34] Kurata, S., Makikawa, M., Kobayashi, H., Takahashi, A., and Tokue, R. “Jointmotion monitoring by accelerometers set at both near sides around the joint.” The20th Annual International Conference of the IEEE Engineering in Medicine andBiology Society. 1998.

[35] Lapadatu, D., Habibi, S., Reppen, B., Salomonsen, G., and Kvisteroy, T. “Dual-axescapacitive inclinometer/low-g accelerometer for automotive applications.” IEEEInternational Conference on Micro Electro Mechanical Systems. IEEE, 2001, 34–37.

[36] Laurens, J. and Droulez, J. “Bayesian processing of vestibular information.” Biologi-cal Cybernetics 96 (2007).4: 389–404.

[37] Lotters, J.C., Schipper, J., Veltink, P.H., Olthuis, W., and Bergveld, P. “Procedurefor in-use calibration of triaxial accelerometers in medical applications.” Sensors andActuators A: Physical 68 (1998).1-3: 221–228.

[38] Luinge, H.J. and Veltink, P.H. “Inclination measurement of human movement using a3-D accelerometer with autocalibration.” IEEE Transactions on Neural Systems andRehabilitation Engineering 12 (2004).1: 112–121.

[39] ———. “Measuring orientation of human body segments using miniature gyroscopesand accelerometers.” Medical and Biological Engineering and Computing 43 (2005).2:273–282.

[40] Macellari, V. “CoSTEL: a computer peripheral remote sensing device for3-dimensional monitoring of human motion.” Medical and Biological Engineeringand Computing 21 (1983).3: 311–318.

79

[41] Mayagoitia, R.E., Nene, A.V., and Veltink, P.H. “Accelerometer and rate gyroscopemeasurement of kinematics: an inexpensive alternative to optical motion analysissystems.” Journal of Biomechanics 35 (2002).4: 537–542.

[42] Mayne, R. “A systems concept of the vestibular organs.” Handbook of sensoryphysiology: The vestibular system 52 (1974).2: 493–580.

[43] Mergner, T. “Modeling sensorimotor control of human upright stance.” Progress inBrain Research 165 (2007): 283–297.

[44] Mergner, T., Schweigart, G., and Fennell, L. “Vestibular humanoid postural control.”Journal of Physiology 103 (2009).3-5: 178–194.

[45] Mergner, T., Schweigart, G., Maurer, C., and Blumle, A. “Human postural responsesto motion of real and virtual visual environments under different support baseconditions.” Experimental brain research 167 (2005).4: 535–556.

[46] Miyazaki, S. “Long-term unrestrained measurement of stride length and walkingvelocity utilizing a piezoelectric gyroscope.” IEEE Transactions on BiomedicalEngineering 44 (1997).8: 753–759.

[47] Moe-Nilssen, R. “A new method for evaluating motor control in gait under real-lifeenvironmental conditions. Part 1: The instrument.” Clinical Biomechanics 13(1998).4-5: 320–327.

[48] Moe-Nilssen, R. and Helbostad, J.L. “Trunk accelerometry as a measure of balancecontrol during quiet standing.” Gait and posture 16 (2002).1: 60–68.

[49] Moeslund, T.B., Hilton, A., and Kruger, V. “A survey of advances in vision-basedhuman motion capture and analysis.” Computer vision and image understanding 104(2006).2-3: 90–126.

[50] Najafi, B., Aminian, K., Loew, F., Blanc, Y., and Robert, P.A. “Measurement ofstand-sit and sit-stand transitions using a miniature gyroscope and its application infall risk evaluation in the elderly.” IEEE Transactions on Biomedical Engineering 49(2002).8: 843–851.

[51] Nebot, E. and Durrant-Whyte, H. “Initial calibration and alignment of low-costinertial navigation units for land vehicle applications.” Journal of Robotic Systems 16(1999).2: 81–92.

[52] Pappas, I.P.I., Popovic, M.R., Keller, T., Dietz, V., and Morari, M. “A reliable gaitphase detection system.” IEEE Transactions on Neural Systems and RehabilitationEngineering 9 (2001).2: 113–125.

[53] Patane, F., Laschi, C., Miwa, H., Guglielmelli, E., Dario, P., and Takanishi, A.“Design and development of a biologically-inspired artificial vestibular system forrobot heads.” IEEE/RSJ International Conference on Intelligent Robots and Systems.vol. 2. IEEE, 2004, 1317–1322.

80

[54] Pope, M.H., Bevins, T., Wilder, D.G., and Frymoyer, J.W. “The relationship betweenanthropometric, postural, muscular, and mobility characteristics of males ages 18-55.”Spine 10 (1985).7: 644.

[55] Poppe, R. “Vision-based human motion analysis: An overview.” Computer Visionand Image Understanding 108 (2007).1-2: 4–18.

[56] Powell, W.B. and Pheifer, D. “The Electrolytic Tilt Sensors.” Sensors Mag (2000).

[57] Rabbitt, R., Damiano, E., and Grant, J. “Biomechanics of the semicircular canals andotolith organs.” The Vestibular System (2004): 153–201.

[58] Rabbitt, R.D. and Damiano, E.R. “A hydroelastic model of macromechanics in theendolymphatic vestibular canal.” Journal of Fluid Mechanics 238 (1992): 337–369.

[59] Rao, A.V. Dynamics of particles and rigid bodies: A systematic approach. CambridgeUniversity Press, 2006.

[60] Rhodes, I. “A tutorial introduction to estimation and filtering.” IEEE Transactionson Automatic Control 16 (1971).6: 688–706.

[61] Roetenberg, D. Inertial and magnetic sensing of human motion. Ph.D. thesis,University of Twente, 2006.

[62] Schuler, A.R., Grammatikos, A., and Fegley, K.A. “Measuring rotational motion withlinear accelerometers.” IEEE Transactions on Aerospace and Electronic Systems 3(1967).3: 465–472.

[63] Shkel, A.M. and Zeng, F.G. “An electronic prosthesis mimicking the dynamicvestibular function.” Audiology and Neurotology 11 (2006).2: 113–122.

[64] Steultjens, M.P.M., Dekker, J., Van Baar, M.E., Oostendorp, R.A.B., and Bijlsma,J.W.J. “Range of joint motion and disability in patients with osteoarthritis of theknee or hip.” Rheumatology 39 (2000).9: 955.

[65] Tahboub, K.A. “Optimal estimation of body angular velocity based on otolith-canalinteraction.” Mediterranean Conference on Control and Automation. 16. IEEE, 2008,848–853.

[66] Tong, K. and Granat, M.H. “A practical gait analysis system using gyroscopes.”Medical engineering and physics 21 (1999).2: 87–94.

[67] Trefethen, L.N. and Bau, D. Numerical linear algebra. Society for IndustrialMathematics, 1997.

[68] Vaganay, J., Aldon, M.J., and Fournier, A. “Mobile robot attitude estimationby fusion of inertial data.” 1993 IEEE International Conference on Robotics andAutomation. 1993, 277–282.

81

[69] van den Bogert, A.J., Read, L., and Nigg, B.M. “A method for inverse dynamicanalysis using accelerometry.” Journal of Biomechanics 29 (1996).7: 949–954.

[70] Veltink, P.H., Luinge, H.J., Kooi, B.J., Baten, C.T.M., Slycke, P., Olthuis, W., andBergveld, P. “The artificial vestibular system-design of a tri-axial inertial sensorsystem and its application in the study of human movement.” Control of Posture andGait, International Society for Postural and Gait Research (2001).

[71] Veltink, P.H., Martens, LJ, and Van Lummel, R.C. “Detection of static and dynamicactivities using uniaxial accelerometers.” IEEE Transactions on RehabilitationEngineering 4 (1996).4: 375.

[72] Vikas, V. and Crane, C.D. “Robot Inclination Estimation using Vestibular DynamicInclinometer.” IASTED International Conference Robotics and Applications. vol. 706.ACTA Press, 2010.

[73] Vukobratovic, M. and Borovac, B. “Zero-moment point-thirty five years of its life.”International Journal of Humanoid Robotics 1 (2004).1: 157–173.

[74] Weinberg, M.S., Wall, C., Robertsson, J., ONeil, E., Sienko, K., and Fields, R. “Tiltdetermination in MEMS inertial vestibular prosthesis.” Journal of BiomechanicalEngineering 128 (2006): 943.

[75] Willemsen, A.T.M., Frigo, C., and Boom, H.B.K. “Lower extremity anglemeasurement with accelerometers-error and sensitivity analysis.” IEEE Transac-tions on Biomedical Engineering 38 (2002).12: 1186–1193.

[76] Willemsen, A.T.M., Van Alste, J.A., and Boom, H.B.K. “Real-time gait assessmentutilizing a new way of accelerometry.” Journal of Biomechanics 23 (1990).8: 859–863.

[77] Williamson, R. and Andrews, B.J. “Detecting absolute human knee angle andangular velocity using accelerometers and rate gyroscopes.” Medical and BiologicalEngineering and Computing 39 (2001).3: 294–302.

[78] Wilson, V.J. and Jones, G.M. Mammalian Vestibular Physiology. Plenum PublishingCorporation, 1979.

[79] Xsens. “http://www.xsens.com/.” MVN Biomech, 2011.

[80] Yotter, R.A., Baxter, RR, Ohno, S., Hawley, S.D., and Wilson, D.M. “On amicromachined fluidic inclinometer.” 12th International Conference on Solid-StateSensors, Actuators and Microsystems. vol. 2. IEEE, 2003, 1279–1282.

[81] Zeng, H. and Zhao, Y. “Sensing Movement: Microsensors for Body MotionMeasurement.” Sensors 11 (2011).1: 638–660.

82

BIOGRAPHICAL SKETCH

Vishesh Vikas was born in New Delhi, India in 1983. He received his bachelor’s degree

in mechanical engineering from Indian Institute of Technology, Guwahati in 2005. After

that he worked at Autonomous Intelligent MAchines (MAIA) Lab, LORIA. He joined the

Center of Intelligent Machines and Robotics (CIMAR), University of Florida, Gainesville

in January 2007. He received his MS in mechanical engineering in summer 2009. He

completed his PhD in mechanical engineering with minors in mathematics and computer

science in fall 2011. His research interests include robotics, artificial intelligence, machine

learning, optimal estimation, nonlinear control, humanoid robots, energy efficient devices

and multi-agent systems.

83