University of Southern California Dissertations and Theses

An approach to experimentally based modeling and simulation of human motion

(USC Thesis Other)

An approach to experimentally based modeling and simulation of human motion

PDF

Download a page range

Download transcript

Copy asset link

Request this asset

Transcript (if available)

Content AN APPROACH TO EXPERIMENTALLY BASED MODELING AND
SIMULATION OF HUMAN MOTION
by
Jenchieh Lee
A Dissertation Presented to the
FACULTY OF THE USC GRADUATE SCHOOL
UNIVERSITY OF SOUTHERN CALIFORNIA
In Partial Fulfillment of the
Requirements for the Degree
DOCTOR OF PHILOSOPHY
(MECHANICAL ENGINEERING)
December 2009
Copyright 2009 Jenchieh Lee
ii
Table of Contents
List of Tables v
List of Figures viii
Abstract ix
Chapter 1: Introduction 1
1.1 RepresentationsofOrientation ......................... 4
1.2 OrientationEstimationProblem ........................ 6
1.3 InterpolationofOrientation ........................... 7
1.4 ModelinginBiomechanics ............................ 9
Chapter 2: Mathematical Preliminaries 11
2.1 Wahba’sProblem................................. 11
2.2 q-Method ..................................... 12
2.3 ConstrainedMinimization-PenaltyFunction ................. 16
Chapter 3: Algorithm Framework 18
3.1 EstimationVariablesandConstraints ..................... 18
3.2 EstimationProcedures.............................. 20
Chapter 4: Constraint Formulation of Kinematics 22
4.1 Definition ..................................... 22
4.2 ConstraintsinVectorForm ........................... 26
SystemIntegrityConstraints .......................... 26
CenterofMassConstraints ........................... 29
AngularMomentumConstraints ........................ 30
Chapter 5: Formulation of Human Body Kinematics 32
5.1 InitialGuessfromMeasurements ........................ 32
InitialGuessofOrientation ........................... 32
iii
InitialGuessofTranslation ........................... 34
5.2 EstimationFormulation ............................. 36
EstimationofAttitude.............................. 36
EstimationofCMPosition ........................... 37
EstimationofAngularVelocity ......................... 38
EstimationofCMLinearVelocity ....................... 39
Estimation of Angular Acceleration . . . . . . . . . . . . . . . . . . . . . . . 40
EstimationofCMLinearAcceleration ..................... 40
Chapter 6: Solutions of Kinematic Estimation 42
6.1 ComputationofOrientation........................... 42
6.2 ComputationofCMPositions.......................... 45
6.3 ComputationofOtherVariables ........................ 48
6.4 KinematicComputationProcess ........................ 50
Self-iteration,Weightingiteration,andStability................ 50
6.5 InterpolationofKinematicVariables ...................... 53
Chapter 7: Dynamic Analysis of Human Body 56
7.1 DynamicEquationFormulation......................... 56
Chapter 8: Algorithm Evaluation: Three Bar Planer Linkage 66
8.1 SchemeSetup................................... 66
8.2 ConstraintMatrixNormalization ........................ 68
8.3 KinematicAnalysis................................ 69
EstimationofPositionandOrientation..................... 69
Changing Weighting Sequence W
1
ofJointConstraint ............ 70
Changing Weighting Sequence W
2
ofCMConstraint ............. 78
Changing Weighting Sequence W
3
ofJointConstraint ............ 79
Changing Weighting Sequences W
1
and W
3
of Joint Constraint . . . . . . . 83
Combination of Weighting Sequences W
1
, W
2
,and W
3
Variation ...... 90
EstimationofLinearandAngularVelocities.................. 96
WeightingCorrectionandInterpolation .................... 101
KinematicFinalResults ............................. 102
8.4 DynamicAnalysis ................................ 112
InverseDynamics................................. 112
Chapter 9: Experiment: 3D Human Jumping Motion 120
9.1 SchemeSetup................................... 120
9.2 ExperimentSetup ................................ 124
9.3 WeightingSetupandDynamicEquation.................... 127
9.4 KinematicEstimationResults.......................... 127
ConstraintsEvaluation.............................. 127
9.5 DynamicEstimationResults .......................... 134
InverseDynamics................................. 134
ForwardDynamics ................................ 137
iv
Chapter 10: Summary of Works 141
Chapter 11: Future Work 144
11.1InterpolationofEstimatedResults ....................... 144
11.2DynamicWeightingSequences ......................... 145
11.3 Three Angle Parameterization forOrientation................. 145
11.4FurtherDevelopmentofAlgorithm ....................... 145
Bibliography 147
AppendixA: Rotation Matrix 157
A.1 Orthogonal Transformation . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
A.2 RotationMatrix ................................. 159
A.3 DerivativeofRotationMatrix.......................... 163
AppendixB: Quaternions 166
B.1 Quaternion Definition .............................. 166
B.2 QuaternionProperties .............................. 167
B.3 RepresentationasExtensionofComplexNumbers .............. 169
B.4 QuaternionProductinMatrixForm ...................... 170
B.5 QuaternionandRotation ............................ 171
B.6 DerivativeofQuaternion............................. 173
AppendixC: Three-Angle Parameterization of Orientation 176
C.1 DefinitionofThreeAngles............................ 176
C.2 EstimationofThreeAngles ........................... 178
C.3 EstimationofFirstDerivativesofThreeAngles................ 184
C.4 EstimationofSecondDerivativesofThreeAngles............... 186
AppendixD: Quaternion Identity and Rotation Matrix 189
D.1 Identities with Arbitrary Vectors . . . . . . . . . . . . . . . . . . . . . . . . 195
D.2 IdentitieswithAngularVelocity......................... 198
AppendixE: Computation of Kinematic Estimation 201
E.1 ComputationofAngularVelocities ....................... 201
E.2 ComputationofLinearVelocitiesofCM .................... 206
E.3 Computation of Angular Accelerations . . . . . . . . . . . . . . . . . . . . . 210
E.4 ComputationofLinearAccelerationofCM .................. 213
AppendixF: Human Body Scheme 216
AppendixG: Human Kinematic Estimation Results 218
AppendixH: Human Dynamic Estimation Results 238
v
List of Tables
F.1 Segmentdimensions ............................... 216
F.2 Inertialradius................................... 217
vi
List of Figures
1.1 The scheme flowchartofproposedalgorithm.................. 10
2.1 Single rigid body orientation estimated in di fferentframes........... 12
2.2 Externalconstrainedminimization........................ 17
3.1 Procedureofestimationframework. ...................... 21
4.1 Vector definitioninrigidbody.......................... 23
4.2 Relationshipsbetweenneighboringbodies ................... 24
4.3 Jointconstraintbetweentwobodies ...................... 24
4.4 Jointlocationconstraint............................. 26
4.5 Jointvelocityconstraint ............................. 28
4.6 Jointaccelerationconstraint........................... 28
4.7 Constraints of center of mass and linear momentum of the system . . . . . 30
4.8 Angularmomentumconstraintofthesystem ................. 31
5.1 Marker position estimated from CM location r ................ 33
5.2 Multi-bodychain ................................. 36
6.1 Iteration process in Algo
p
............................ 51
6.2 Concept of self-iteration and weighting iteration of attitude estimation. . . 51
6.3 Concept of self-iteration and weighting iteration of other variables. . . . . . 52
6.4 Three-angleparameterization........................... 54
7.1 Variable definitionsindynamicequationformulation.............. 57
7.2 Estimationofgeneralizedforce ......................... 64
8.1 Schemesetupofthreebarlinkagesystemin2Dplane. ............ 67
8.2 Positionsofmarkersofeachsegment....................... 67
8.3 Error types introduced in three bar linkage simulation. . . . . . . . . . . . . 70
8.4 Joint constraints with ∆ y=0 .1 m and ∆ θ=0deg .............. 72
8.5 Results of weighting iteration process. ∆ y=0 .1 m, W
1
= dx·5
kn −1
.... 73
8.6 Joint constraint under weighting sequence W
1
= dx·2
kn −1
.......... 74
8.7 Convergence under di fferent W
1
. ∆ θ=10degrees. .............. 75
8.8 Gap convergence. ∆ θ=10 , W
1
= dx·5
kn −1
................. 76
8.9 Gap convergence. W
1
= dx·5
kn −1
....................... 77
8.10 Di fferent W
2
convergences for joint gaps. ∆ y=0 .01 m............ 78
vii
8.11 Convergence with W
3
weightingsequence. .................. 80
8.12 Weighting results for gaps, W
3
=0 .01×2
kn −1
................ 81
8.13 Convergence for di fferent ∆y. .......................... 82
8.14 Gap under di fferent W
1
and W
3
........................ 85
8.15 Local area magnificationinFig.8.14. ..................... 86
8.16Properweightingsequencecombination .................... 87
8.17 Convergence for W
1
W
3
combination ...................... 88
8.18Finalshaperesultscomparison. ........................ 89
8.19 Setup of errors: ∆ x=0 .1 m , ∆ θ=10deg ................... 91
8.20Linkageshapecomparison ............................ 92
8.21 Magnifiedjointlocationsandendpoints. ................... 93
8.22 Gap correction under sets of weightings . . . . . . . . . . . . . . . . . . . . 94
8.23 Estimation of system’s CM location under di fferent W
2
sequences. . . . . . 95
8.24Finalestimationofthesystem. ......................... 96
8.25Theconceptofjointvelocity .......................... 97
8.26 Angular momentum di fference estimation, V
1
set................ 97
8.27 Angular momentum di fference estimation, V
4
set................ 98
8.28 Joint 1 velocity di fference, V
2
and V
5
sets. .................. 99
8.29 Joint 2 velocity di fference, V
2
and V
5
sets. ................... 100
8.30 Relationship between nominal reference, inital guess, and estimation results. 102
8.31 Angle q
1
estimation. ............................... 105
8.32 Angle q
2
estimation. ............................... 106
8.33 Angle q
3
estimation. ............................... 107
8.34Joint1estimation ................................ 108
8.35Joint2estimation ................................ 109
8.36SystemCM.estimations ............................. 110
8.37Angularmomentumestimation ......................... 111
8.38 Torque τ
1
estimation results in di fferentscales ................ 113
8.39 Torque τ
2
estimation results in di fferentscales ................ 114
8.40 Torque τ
3
estimation results in di fferentscales ................ 115
8.41 Force p
x
estimation results in di fferentscales ................. 116
8.42 Force p
y
estimation results in di fferentscales ................. 117
8.43 Torque τ
2
estimation results with velocity and acceleration measurements . 118
8.44 Torque τ
3
estimation results with velocity and acceleration measurements . 119
9.1 Experimentaldatacollectionsetup ....................... 122
9.2 ShankjointandCMlocationsdetermination ................. 123
9.3 Trunk joint and CM locations determination, frontal view . . . . . . . . . . 124
9.4 Trunk joint and CM locations determination, lateral view . . . . . . . . . . 125
9.5 Humanjumpingexperimentsetup. ....................... 126
9.6 Anklejointlocationconstraintevaluation ................... 129
9.7 Hipjointvelocityconstraintevaluation..................... 130
9.8 Shoulder joint acceleration constraint evaluation. . . . . . . . . . . . . . . . 131
9.9 SystemCMevaluation.............................. 132
9.10 Angular momentum and external torque evaluation . . . . . . . . . . . . . . 133
viii
9.11Forcesactingontrunk .............................. 135
9.12Torqueactingonkneejoint............................ 136
9.13 Frame 1 in flighttask. .............................. 138
9.14 Frame 3 in free flighttask............................. 139
9.15 Frame 5 of free flighttask............................. 140
A.1 Coordinaterotation ............................... 159
B.1 Rotationvectordecomposition ......................... 173
C.1 Three-angleparameterization........................... 177
C.2 Non-uniquerepresentationsofsamedirectionvector. ............. 180
C.3 Direction unit vectors of two successive time frame . . . . . . . . . . . . . . 181
C.4 Angle sweep di fferences ............................. 182
C.5 x − zplanprojectionofdirectionvector .................... 182
C.6 Special case of direction vector . . . . . . . . . . . . . . . . . . . . . . . . . 183
G.1 Anklejointlocationconstraints ......................... 219
G.2 Kneejointlocationconstraints ......................... 220
G.3 Hipjointlocationconstraints .......................... 221
G.4 Wristjointlocationconstraints ......................... 222
G.5 Elbowjointlocationconstraints......................... 223
G.6 Shoulderjointlocationconstraints ....................... 224
G.7 Neckconstraints ................................. 225
G.8 Anklejointvelocityconstraints ......................... 226
G.9 kneejointvelocityconstraints.......................... 227
G.10Hipjointvelocityconstraints .......................... 228
G.11Wristjointvelocityconstraints ......................... 229
G.12Elbowjointvelocityconstraints......................... 230
G.13Shoulderjointvelocityconstraints ....................... 231
G.14Anklejointaccelerationconstraints....................... 232
G.15Knee joint acceleration constraints . . . . . . . . . . . . . . . . . . . . . . . 233
G.16Hip joint acceleration constraints . . . . . . . . . . . . . . . . . . . . . . . . 234
G.17Wrist joint acceleration constraints . . . . . . . . . . . . . . . . . . . . . . . 235
G.18Elbowjointaccelerationconstraints ...................... 236
G.19Shoulderjointaccelerationconstraints ..................... 237
H.1 Externalforcesactingontrunk ......................... 239
H.2 Torquesinanklejoints.............................. 240
H.3 Torquesinelbowjoints.............................. 241
H.4 Torquesinhipjoints ............................... 242
H.5 Torquesinkneejoints .............................. 243
H.6 Torquesinneckandontrunk. ......................... 244
H.7 Torquesinshoulderjoints ............................ 245
H.8 Torquesinwristjoints .............................. 246
ix
Abstract
A computational approach to estimation of the kinematic of a mechanical system
consisting of interconnected rigid bodies performing motion in 3-dimensional space is de-
veloped. Themainapplicationconsideredinthisdissertationisanexperimentally-based
analysis of human motion. It is assumed that an experimentally measured motion data
in the form of marker locations is provided. Using the experimental data, the proposed
computational procedure computes the entire kinematics of the system while preserving
important physical and dynamic properties. These properties include systm integrity, and
whole-body dynamic properties such as the system’s center of mass kinematics, and its
angular momentum.
Theproblemoffindingtheorientationofinterconnectedrigidbodiesisformulated
asaleastsquaresproblem. Constraintsusedtoensurethepreservationoftherequiredprop-
erties are included in the form of penalty functions. Equations for the optimal solution in
the form of quaternions are derived. Since the resulting equations are highly nonlinear,
embedded iteration process to compute the optimal solution was developed and imple-
mented. A quaternion based dynamic equations for a chain of rigid body was formulated
and implemented. An interpolation procedure for attitude given in terms of quaternion
parameterization was developed and implemented.
x
Two simulation studies are performed. To evaluate the numerical e fficiency of
the proposed algorithm, estimation of the kinematics of a chain simulated on a computer is
performed. Toevaluatetheperformanceoftheproposedapproach, a14-bodymodelisused
to estimate human three-dimensional motion during jumping using experimental data. The
results of the two studies indicate fast convergence of the algorithm to an optimal solution
while satisfying the imposed the constraints.
1
Chapter 1
Introduction
Accuratemodelingofthree-dimensionalhumanmovementsiscrucialfordetermin-
ing the torques required to generate the motion, for computing the loads on various parts
of the body, and for understanding the control logic generated by the nervous system.
Modeling of human motion has been studied in both computer science for devel-
opment of animation software, and biomechanics for human body dynamic and kinematic
analysis. In both fields, modeling of human motion is usually based on experimental mea-
surements which obtained time history of the locations of markers placed on various parts
of the body (segments). Marker locations are used to calculate orientation and position of
each segment.
In animation studies, the captured motion is used to create animation character
gesture. Kinematic and dynamic properties areusuallynotthemainconcerninanimation,
and the dynamic equations of motion need not to be formulated. Although proper interpo-
2
lation [28][21][19][7] between captured time frames can be applied to generate smooth and
realistic segment movements, the motion usually does not obey physical laws.
In biomechanics, kinematic and dynamic properties are important to estimate
torques of body joints and understand the control logic of physical human motion. In exist-
ing methods of estimation of human motion [1][63], kinematic computation of each segment
is performed independently. Consequently fitting techniques and filters [39][58][26][29] are
often utilized to eliminate noise and measurement errors. However, independent kinematic
estimation of each segment often leads to non-physical behavior, such as separation of
neighboring segments, and contradiction of laws of motion imposed on whole body kine-
matics. Moreover, in order to simplify analysis [1][17], the motion of the segments is often
assumed to be two-dimensional (2D). That is, the relative rotations between the segments
have fixed axes that are parallel to each other. Although for some joints and motions 2D
rotation model is su fficiently accurate, in general a 2D joint rotation assumption is often
not adequate when the whole body motion is considered.
In this study an approach is presented for the computation of human motion
in 3D space using branched chain of rigid bodies as the model of whole human body. The
computationintendstopreservetheintegrityoftheinterconnectionsandsatisfywholebody
dynamic laws. For example, in free flight tasks total body linear and angular momenta are
conserved. These relations impose constraints on the location of the center of mass (CM)
of each segment and its linear and angular velocities and accelerations.
The proposed approach is formulated as an iterative computational procedure.
Orientation, CM location, linear and angular velocities and accelerations of each segment
3
are estimated. For orientation, Euler symmetric parameters (quaternions) are used for
parameterization of the orientation in 3D space. For CM location, angular and linear
velocity and acceleration of each segment, Cartesian coordinate system is used.
The orientation estimation part of the proposed algorithm is based on Wahba’s
problem [68], which was formulated for attitude estimation of a single rigid body such as
a spacecraft. In aerospace, the attitude of a spacecraft with respect to a fixed coordinate
system is needed for navigation and attitude control purposes. A solution of Wahba’s
problem by Davenport [9], called q-method, uses quaternion as the estimated variable.
For multi-segment dynamic model discussed here, Wahba’s problem was reformulated for
multiple bodies and the constraints imposed on the system are included in the formulation.
Also,anewsolutionfromq-methodisrederivedtocooperatewiththereformulatedWahba’s
problem.
For each segment, the estimations of orientation and position are based on initial
measurements of markers, which serve as initial guesses, and kinematic constraints are
also included in the estimation process. These constraints include the trajectory of CM
of the whole body, linear and angular momenta of the whole body, linear acceleration and
externaltorqueappliedonthebody,andjointlocations,velocities,andaccelerations. These
constraints are incorporated in the iteration process via penalty function method. In this
fashionofiteration,the final estimation satisfies simultaneously all the constraints.
4
1.1 Representations of Orientation
Orientationofanobjectcanberepresentedindi fferent ways, such as Direction
Cosine Matrix (DCM), Euler angles, axis/angle representation, quaternions, and Rodrigues
(Gibbs vector) representation.
Direction Cosine Matrix(DCM) is a 3×3 matrix which rotates a vectoraboutone
axis with an angle in the same frame, or relatively speaking, transforms a vector represen-
tation from one frame to another. These two frames have orthogonal unit basis, and DCM
is also orthogonal. The advantage of DCM is that it is straightforward to understand. A
DCM matrix, R, has nine parameters, and because of the constraint R
T
R = I,whicha
DCMmust satisfy (six equalities), everyrotationcanbe writteninterms of minimum three
parameters. Thus the disadvantage of DCM is its excessive over-parameterization.
Euler angle parameterization [14] is defined by the angles of a sequence of three
rotations about the axes of a body-fixed frame. The advantages of the Euler angle para-
meterization are, its minimality (only three parameters are needed), ease of physical inter-
pretation, especially in engineering design and biomechanics. The disadvantages of Euler
representation include ambiguity since one needs to select one of twelve possible sequences,
heavy computation loads since 27 multiplications are needed to compute a rotation, and
singularity of the parameterization also known as gimbal lock
Axis/angle representation contains a vector and an angle. It stems from the fact
that any rotation matrix has a vector that is unchanged by the rotation, and the angle
denotes the angle of rotation around this vector.
5
Quaternions were proposed by W.R. Hamilton [18] and applied to mechanics in
three-dimensional space. A quaternion contains four quantities, also known as Euler sym-
metric parameters. One of the advantages of the quaternion representation is that it avoids
discontinuousangleestimation. Furthermore, thecomputationloadisreducedbecauseonly
16 multiplications are needed compared to 27 multiplications in Euler angle representation.
Closely related to the Euler symmetric parameters is another attitude representa-
tion, the Rodrigues vector, or Gibbs vector[59]. This parameterization possessed a singu-
larity when rotation angle is π.
In this study, the quaternion parameterization is used. This is because in many
applications, such as whole body motion system undergoes large rotations and quaternion
representationavoidssingularitiespresentin3-dimensionalparameterizations. Also,quater-
nion is computationally e fficient, and the problem of orientation estimation can be directly
formulated in terms of quaternions.
The quaternion is defined as follows:
¯ q =
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(1.1)
where
q =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=sin
µ
θ
2
¶
· ˆ n, q
4
=cos(
θ
2
) (1.2)
the vector ˆ n is the direction of rotation axis, and θ is the angle of rotation.
6
1.2 Orientation Estimation Problem
Attitude estimation problem arose in aerospace field for spacecraft maneuver and
attitude control. Attitude determination from point data and vector observation is studied
for this purpose [60][8][57][47][66]. For a single rigid body, the optimal attitude determi-
nation problem has been proposed by Wahba [68]. Based on Wahba’s work, Davenport
[9] derived an elegant algorithm to solve Wahba’s problem in terms of quaternion, called
the q-method [27]. Shuster presented an algorithm called QUEST [60][61], which solved
the eigenvalue problem in q-method without finding all the eigenvalues. This increased
the speed of the attitude determination calculation and was useful when attitude needs to
be estimated frequently. Rapid maneuver of a spacecraft in space is one example of this
demand. After QUEST has been presented, Markley [32][35][36] and Mortari [41][43] de-
veloped several algorithms to further increase the computation speed, such as ESOQ [42]
and ESOQ2 [44][45] algorithms. Extensive survey in discussing spacecraft attitude deter-
mination and control is given in [69]. Several other researches [37][38][3][66] and surveys
[59] were related to spacecraft attitude determination and attitude representation as well.
In addition to QUEST, Horn proposed a similar solution in closed-form using both ortho-
normal matrices(or DCM) and unit quaternions [23][24]. While QUEST was developed for
spacecraftattitudedetermination[30][15][46][56],thefocusforHorn’swork[25]wasonclas-
sic photogrammetric tasks. Although these two types of problems seem to be in di fferent
fields, the main idea was the same: finding the optimal relationshipbetweentwo coordinate
systems using vector observation.
7
Extended QUEST algorithm were developed by Markley [33][34] who attempted
toextendQUESTtoincludeestimationofotherparameters. Psiaki[50]triedtogeneralized
the solution for this type of quadratic functions. Bar-Itzhack modified QUEST to consider
all the past measurements into account [2] while the original QUEST algorithm only took
the measurements of vectors in a single time frame. QUEST has also been extended to
performwellwithextendedKalman filter (EKF)[39][5][55][58][51][4][31]. Although Kalman
filters perform well in incorporating system dynamic models, an extended Kalman filter
can be sensitive to the initial attitude guess and can even diverge [51]. Psiaki utilized a
new spacecraft attitude determination filter to estimate the attitude more e fficiently and
correctly, and it out-performed EKF. Dual quaternion method is another way to increase
the estimation accuracy considering simultaneously translation and rotation[12].
In biomechanics, orientation estimation problem occurs when loads on segments
needtobedetermined. Whensegmentorientationsareestimatedviamarkerlocations[62][63]
for a serious of time frames, human motion can be analyzed and loads and torques in body
joints can be evaluated from kinematic and dynamic estimation. In this work, this is the
main application of the proposed estimation method.
1.3 Interpolation of Orientation
For all the algorithms related to QUEST, they only calculate the optimal attitude
of one rigid body in one single time frame. For spacecrafts, this is su fficient for control sys-
tem development. In other fields, such as computer graphics, animation, and biomechanics,
discrete attitude information is not su fficient. Interpolation of attitudes is needed in order
8
to generate a smooth trajectory in animation, or to be able to compute system behavior in
biomechanic applications.
Quaternion of orientation is a good candidate for interpolation of orientation be-
cause it does not have singularity at certain angles of rotation. A number of quaternion
interpolation methods have been proposed. Spherical linear interpolation (SLERP) was the
first interpolation method of quaternion, was introduced by Shoemake [54] and an interest-
ing proof is given by Glassner [16]. SLERP provides constant speed interpolation between
two quaternions resulting in quaternion interpolation
A C
2
-continuous B-spline quaternion curve interpolation was proposed by M.J.
Kim and M.S. Kim in 1995 [28]. It solved the problem of C
2
-discontinuity of the B-
spline quaternion curves in SO(3). Other researchers,(seeF.C.ParkandB.Ravani[49]),
recommended smooth invariant interpolation of the rotation group SO(3), i.e. DCM. Their
methods minimized approximately angularacceleration. From the mid 1990’s up to present
time, these interpolation methods have been further developed[20][21][40][19][7].
In biomechanic applications, kinematic interpolation is also important. J. Coburn
and J.J. Crisco [7] used Hermite curves to interpolating three-dimensional kinematic data.
Even with a small size of data sample, their interpolation method can still present the
results of a large matrix data. A closed form solution for C
2
orientation interpolation was
proposed by V. Volkov and L. Li [67], and the significance of this method is that it does
not rely on cubic B-spline blending functions, but using C
2
interpolatory (cardinal) basis.
With di fferentiation of the trajectories of quaternion [6], C
2
-continuous interpo-
lations reveal the information of estimated velocity and acceleration, thus a better un-
9
derstanding of dynamic properties of a system. In biomechanics, the estimated dynamic
properties are the keys to understand the forces and torques in human body joints. Human
motion analysis [1] can be separated into three stages: body structure analysis [63], track-
ing [72], and recognition [65][22]. In the robotic field, control is the stage after recognition
[52][13]. In robot kinematics, quaternion position representation was proposed by some
other researchers [64][11], and control theories were also applied to control manipulators or
robots in quaternion fashion [70].
Although the continuous function of quaternion can be obtained by interpolation
methods mentioned above, some of the methods might have turning-over between consecu-
tive frames. In this work a newinterpolation method which separates the rotation direction
vector and the rotation angle of quaternion is proposed. In this method turning-over can
be avoided.
1.4 Modeling in Biomechanics
Themotivationofthisworkisinsearchingforarigidbodysystemthatisclosedto
realhumanbodymotion. Propertiesofhumanbodysegmentshavebeenstudiedtoquantify
human movements[76][74][73][75][10]. Using dynamic equations of rigid bodies, the forces
and torques in the rigid body model can be calculated from kinematic measurements. The
calculation of torques and forces can then be evaluated as the torques and forces in human
body joints[1][53].
In this work a model of three bar linkage of rigid bodies is first evaluated using
proposed algorithm, and the setup is similar to human jumping motion. Later real experi-
10
mental human jumping data is evaluated to estimate possible forces and torques acting in
human body. Figure. 1.1 shows the flowchartofestimationinthiswork. Theproposedal-
gorithm is performed as a measurement filter which estimates possible solutions around the
initial guess q
∗
neighborhood to satisfy constraints. In Fig. 1.1, measurement filter is the
algorithm containing 3 di fferent estimations: position estimation, velocity estimation, and
acceleration estimation. For each estimation, only the constraints related to the variables
are included. For example, position estimation includes only joint gaps and CM location
constraints,andvelocityestimationincludesconstraintsofjointvelocities,CMvelocity,and
angular momentum.
Nervous
System
Human
Musculo-
skeletal
System
output
q*
Input
q
Measure
Filter:
Inverse
Dynamics
Inverse
Kinematics
motion command
output
Coord.
Transform
Position
estimation
Velocity
estimation
Acceleration
estimation
Forces
and
Torques
input
θ
*
θ
Figure 1.1: The scheme flow chart of proposed algorithm.
11
Chapter 2
Mathematical Preliminaries
2.1 Wahba’s Problem
In 1965, Wahba [68] proposed an optimization problem to estimate the attitude of
a single rigid body via vector observation on a rigid body. From Fig. 2.1, Wahba’s problem
states as follows:
min
R
N
X
s=1
X
s
k b
s
− Ra
s
k
2
(2.1)
subj ested to R
T
R = I
where
X
s
− weighting coe fficient of s
th
pair of measurements.
N − number of vector observations.
a
s
− representation of p
s
measured in body frame { B}.
b
s
− representation of p
s
measured in reference frame { I}
12
R = R
B
I
is the rotation matrix transforming vector presentations given in frame
{ B} to those in frame { I}.
X
Y
Z
x
y
z
{B}
p
s
{I}
Figure 2.1: Single rigid body orientation estimated in di fferent frames.
In absence of measurement errors, the following equation holds
R
B
I
( p
s
)
B
= R
B
I
a
s
=( p
s
)
I
= b
s
(2.2)
Since errors are usually present, the objective is to find the orthogonal matrix R that min-
imizes the weighted sum of square of the di fference between the measured and transformed
vectors. In aerospace application, the vector observation is in the body frame. That means
b
s
is known and a
s
is from observation. In biomechanic application, a
s
is known and b
s
is
from observation. It is assumed that the measured vectors are linearly independent, and
N> 2.
2.2 q-Method
The q-method was devised by Davenport [9] to solve Wahba’s problem given as
Eq.(2.1). The q-method transforms the minimization problem into an eigenvalue problem
13
as follows:
min
R
N
X
s=1
X
s
k b
s
− Ra
s
k
2
=min
R
N
X
s=1
°
°
°
p
X
s
b
s
− R
p
X
s
a
s
°
°
°
2
=min
R
N
X
s=1
k B
s
− RA
s
k
2
=min
R
N
X
s=1
¡
B
T
s
B
s
+ A
T
s
R
T
RA
s
−2 B
T
s
RA
s
¢
=min
R
N
X
s=1
³
| B
s
|
2
+| A
s
|
2
−2 B
T
s
RA
s
´
(2.3)
where
p
X
s
b
s
= B
s
p
X
s
a
s
= A
s
Thecondition R
T
R = I isenforced. Thevectors, B
s
and A
s
,aremeasuredinframe
{ I} and { B} respectivelyandare treatedas constants. Thenthe minimizationproblemcan
be rewritten as a maximization problem as follows:
max
R
N
X
s=1
B
T
s
RA
s
(2.4)
R
T
R = I
The rotation matrix R can be expressed in terms of a quaternion, yielding an
optimization problem that yields the optimum quaternion estimation as follows:
J( R)=
N
X
s=1
B
T
s
RA
s
≡ tr( B
T
RA) (2.5)
14
where the (3× N)matrices B and A are defined by
B ≡ [ B
1
.
.
. B
2
.
.
.···
.
.
. B
N
]
A ≡ [ A
1
.
.
. A
2
.
.
.···
.
.
. A
N
]
and the relationship between rotation matrix R and its corresponding quaternion q is as
the following:
q =
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
R( q)=( q
4
−q·q) I
3
+2qq
T
−2 q
4
[[q]] (2.6)
where [[·]] is defined as skew-symmetric matrix operation
[[q]] =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
0 − q
3
q
2
q
3
0 − q
1
− q
2
q
1
0
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(2.7)
Then J( R) can be written as:
J( q)= q
T
Kq (2.8)
where the (4×4)matrix K is
K =
⎡
⎢
⎢
⎣
S − I
3
σZ
Z
T
σ
⎤
⎥
⎥
⎦
(2.9)
15
W = BA
T
S = W
T
W
Z =( W
23
− W
32
,W
31
− W
13
,W
12
− W
21
)
T
σ = tr( W)
In order to find the maximum of Eq.(2.4), quaternion has been substituted as a
new variable with the constraint
k qk
2
=1 (2.10)
Using Lagrange multiplier, the objective function is then
J
0
( q)= q
T
Kq − λq
T
q (2.11)
Maximum value of J
0
( q) can be found by partial di fferentiation as follows
∂J
0
∂q
=2(Kq − λq)=0 (2.12)
Kq = λq (2.13)
q
T
q=1 (2.14)
Hence q is the eigenvector corresponding to the largest eigenvalue, λ
max
,ofmatrix
K to have the extreme value. Therefore in Eq.(2.8), J( q)= q
T
Kq = λ
max
.
16
2.3 Constrained Minimization - Penalty Function
Consider nonlinear programming problem
Minimize f( x) (2.15)
where ( x)=( x
1
,x
2
,··· x
n
)
T
is the vector of the design parameters subject to the inequality
(regional) constraints
g
i
( x) ≤ 0 i=1 ,2 ,··· p
(2.16)
Thus, if the vector ( x
∗
) corresponds to the minimum (optimal ) value of the func-
tion,
f( x
∗
)=min
x ∈ Z
f( x) (2.17)
where the domain Z is determined by the constraints.
The penalty method gradually increases the weighting of the constraint function,
and a feasible solution can be found. Here only the exterior penalty function is introduced.
An objective function can be formed as
U( x)= f( x)+
1
r
p
X
i=1
[ g
i
( x)]
2
(2.18)
The objective is now to find the minimum of function U( x). g
i
( x) is set to be
equal to zero if the inequality is satisfied. In this case the unconstrained minimum lies in
the nonfeasible region, as shown in Fig. 2.2. As r< 1 and decreases, modified function
creates a series of suboptimal points that approach the constraint boundary from outside
thefeasibleregion.
17
Unconstrained
minimum
Constrained
minimum
r
=
r
3
r
=
r
2
r
=
r
1
Feasible
region
Nonfeasible
region
U(x)
x
Figure 2.2: External constrained minimization.
18
Chapter 3
Algorithm Framework
3.1 Estimation Variables and Constraints
In this study there are six sets of variables for estimation:
(1)quaternion of orientation of each body, q
i
.
(2)angular velocity of each body, ω
i
.
(3)angular acceleration of each body, α
i
.
(4)CM position of each body, r
ci
.
(5)CM velocity of each body, ˙ r
ci
.
(6)CM acceleration of each body, ¨ r
ci
.
And also, eight constraints of kinematics of a multi-body system are incorporated:
(a) system integrity, including joint location, velocity, and acceleration between
two neighboring segments,
(b) CM constraints, including CM position, velocity, and acceleration,
19
(c) angular momentum constraints, including the value and the rate of change of
angular momentum of the system.
The general form of this algorithm for each set of variable is based on penalty
function method with common structure as follows
min
x
U( x)=min
x
f( x)+
n
X
i=1
W
i
· g
i
( x) (3.1)
where xisthevariabletobeestimated, f( x) is the estimating function, g
i
( x) is the con-
straint function and W
i
is the weighting related to the constraint. In this study, six sets of
variablesareestimatedasmentionedabove. Theconstraintsareintheformoflossfunction,
that is, the square of norm of a vector constraint, and then U( x) is in quadratic form. After
partial di fferentiation of U( x) and collecting variables, the function can be in the form as
follows
K(W, x
∗
)· x = λx (3.2)
M · x = F(W, x
∗
) (3.3)
Forestimationofquaternionoforientation,Eq. (3.2)issimilartoEq. (2.13)exceptnowthe
K matrix containsthe information ofinitial guess x
∗
andweighting W, whichisa collective
set of all W
i
related to the constraints associating with orientation of quaternion. W
i
is
defined as a sequence of weighting coe fficients. For other sets of variables, Eq. (3.3) is the
form, where M isanon-singularconstant matrix, and F isavectorcontainingweighting W
and initial guess x
∗
. For both equations, with initial guess and first weighting coe fficient in
W
i
sequence,newestimationof x
es t
canbefoundanditeratedbackuntilconvergence. Thus
there are two iteration loops: one is for weighting coe fficient sequences, and the other is
20
for new estimated variables. The two iteration processes are discussed in section 6.4. After
the two iteration processes, the final estimation of each variable satisfies all the constraints
proposed above.
3.2 Estimation Procedures
Figure3.1showstheprocedureoftheframeworkofthisstudy. Positionestimation
algorithm, denoted as Algo
p
in Fig. 3.1, evaluates positions and orientations from marker
locations. Similarly, velocity estimation algorithm and acceleration estimation algorithm
are denoted as Algo
v
and Algo
a
respectively. Each estimation algorithm is acting as a filter
searching solutions around the initial guess neighborhood. Algo
p
includes two equations:
CM position estimation equation and orientation estimation equation. The constraint of
jointgapiscoupledinthesetwoequations, andthusthetwoequationsneedtobeevaluated
together. Similarly, equations of velocities are evaluated in Algo
v
and equations of accel-
erations are evaluated in Algo
a
.TheresultsofAlgo
p
are CM positions and quaternions
that satisfy position-related constraints, and the estimation set is denoted as P
1
. sp
p
is the
spline interpolation for position estimation results P
1
, and consequently the initial guesses
ofvelocitiesareobtainedfrom sp
p
forvelocityestimationsinAlgo
v
. [ P
1
,V
1
,A
1
]aretheesti-
mationsofpositions,velocities,andaccelerationsfromspline sp
p
. TheinputofAlgo
v
are P
1
and V
1
, and the estimation results are CM velocities and angular velocities, denoted as V
2
,
which, combining P
1
,satisfies velocity-related constraints. sp
v
is the spline interpolation of
V
2
,and A
2
is the initial guesses of accelerations for Algo
a
. P
2
is the position estimation by
integrating sp
v
, which might not satisfy position-related constraints. For Algo
a
, the input
21
are [ P
1
,V
2
,A
2
], and the result is A
3
, which is a set of accelerations satisfying acceleration-
relatedconstraints. sp
a
is the spline interpolationofaccelerationestimationresults A
3
,and
P
3
and V
3
are the position and velocity estimation from integration of sp
a
.
At the end of estimation process, [ P
1
,V
2
,A
3
] is the set of position, velocity and
acceleration that satisfies all the constraints, thus it’s used to estimate forces and torques
in body joints. If marker velocity and acceleration measurements are available, they can
also serve as initial guesses for algorithms. In the following chapters, the formulation of
constraints are illustrated, and later on these are incorporated with the estimations of
variables in concern.
Marker
Position
Algo
f(q ),f(r )
p
ici
Algo
f( ),f(r )
v
ω
ici
.
Algo
f( ),f(r
a
α
..
)
ici
q
r
i
ci

est
est
ω
i
ci

r
e

st
est
.

r
α
..
i
ci
est
est
Spline
sp
p
Spline
sp
v
Spline
sp
a
P
1
P
2
P
3
V
1
V
2
V
3
A
1
A
2
A
3
Marker
Velocity
Marker
Acc.
P
1
P
1
V
2
Figure 3.1: Procedure of estimation frame work.
22
Chapter 4
Constraint Formulation of
Kinematics
4.1 Definition
ConsiderarigidbodyshowninFig. 4.1, { B}denotesaframeattachedtothebody,
and { I} denotes reference frame. Let P be an arbitrary point on the body representing
the position of a marker attached to the body. Point P can also denote a special point
of interest, such as center of mass of the rigid body, or the average point of all markers
on the body. The location of point J is given in the body coordinate and its location in
the reference coordinate system needs to be determined. In this study, J denotes a 3D
rotational joint. The definition of a joint in this application is a connecting point between
two rigid bodies, and there is no relative translation of these two rigid bodies at this point.
23
Let ρ and D denote vectors representing the locations of points J and P with
respect to the origin , and let r denotes the location of P with respect to J,theninFig.
4.1 we have
ρ
ref
= D
ref
+ r
ref
= D
ref
+ R· r
bo dy
(4.1)
The subscripts ref and body indicate the frame in which the vector is represented, and R
denotes the rotation transformation from body frame { B} to the reference frame { I}.If J
X
Y
Z
x
y
z
{B}
{I}
D
P
r
J
ρ
Figure 4.1: Vector definition in rigid body
is the joint connects i
th
and j
th
rigid bodies, then as shown in Fig. 4.2
ρ
i
j
= D
i
+ R
i
· r
i
j
(4.2)
where
ρ
i
j
− joint location vector expressed in frame { I}.
D
i
− position vector of a point on body i expressed in frame { I}.
24
X
Y
Z
{I}
D
i
P
i
r
i
j
ρ
i
j J
{j}
{i}
{k}
ρ
i
k
r
i
k
Figure 4.2: Relationships between neighboring bodies
R
i
− a matrix transforming a vector in i
th
body frame to frame { I}.
r
i
j
− joint J location vector with respect to point P
i
{i}
{I}
{j}
ρρ
i
j
=
j
i
D
j
D
i
J
r
j
i
r
i
j
Figure 4.3: Joint constraint between two bodies
The location of joint J can also be expressed with vector r
j
i
in frame { j} as shown
in Fig. 4.3.
ρ
j
i
= D
j
+ R
j
· r
j
i
(4.3)
25
Thus we have the obvious relation
ρ
i
j
= ρ
j
i
(4.4)
26
4.2 Constraints in Vector Form
System Integrity Constraints
The estimations of orientation, CM location, linear and angular velocities and
accelerations of each body in a multi-body system are evaluated independently, thus the
integrity of the system can be compromised during the estimation process. From Eqs. (4.2)
and (4.3), the joint location, velocity, acceleration constraints can be formulated as seen in
Fig. 4.4-4.6 and they can be formulated as follows:
r
ci
+ R
i
o
L
ij
= r
cj
+ R
j
o
L
ji
(4.5)
˙ r
ci
+
˙
R
i
o
L
ij
= ˙ r
cj
+
˙
R
j
o
L
ji
(4.6)
¨ r
ci
+
¨
R
i
o
L
ij
=¨ r
cj
+
¨
R
j
o
L
ji
(4.7)
where r
ci
, ˙ r
ci
,and ¨ r
ci
denote CM position, velocity, and acceleration of i
th
body. R
i
o
denotes
{i}
{I}
{j}
r
cj
r
ci
L
ij
L
ji
Figure 4.4: Joint location constraint
the transformation from i
th
body frame to reference frame. And we have
27
˙
R
i
o
=[[ ω
i
]] R
i
o
(4.8)
¨
R
i
o
=
d
dt
¡
[[ ω
i
]] R
i
o
¢
(4.9)
=[[ α
i
]] R
i
o
+[[ ω
i
]][[ ω
i
]] R
i
o
(4.10)
where
[[ ω
i
]] =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
0 − ω
iz
ω
iy
ω
iz
0 − ω
ix
− ω
iy
ω
ix
0
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(4.11)
ω
i
−Angularvelocityof i
th
bodyrelativetoreferenceframerepresentedinreference
frame.
α
i
− Angular acceleration of i
th
body relative to reference frame represented in
reference frame.
L
ij
− Vector in i
th
bodyframefromCMof i
th
body to the position of joint related
to j
th
body.
Except for L
ij
vector, all other vectors are expressed in reference frame.
For each joint, three constraints are formulated with respect to joint position,
velocity, and acceleration as listed in Eqs. (4.5 - 4.7).
28
{i}
{I}
{j}
L
ij
L
ji
r
.
ci
r
.
cj
ω
i
ω
j
Figure 4.5: Joint velocity constraint
{i}
{I}
{j}
L
ij
L
ji
r
..
ci
α
i
r
..
cj
α
j
Figure 4.6: Joint acceleration constraint
29
Center of Mass Constraints
Given the net force acting on the system and initial conditions, the location, ve-
locity and acceleration of the center of mass at every instant of time are determined and
can serve as constraints as follows:
Mr
c
=
n
X
i=1
m
i
r
ci
(4.12)
M ˙ r
c
=
n
X
i=1
m
i
˙ r
ci
(4.13)
M¨ r
c
=
n
X
i=1
m
i
¨ r
ci
=
n
X
i=1
F
i
(4.14)
where n is the number of segments, M is the total mass of the system, and m
i
is the mass
of the i
th
body. F
i
is the net force acting on i
th
body. For free flight case, the net force
acting on the system is gravity force, which is a constant.
The vector representations above are all expressed in reference frame. If there is
no external force acting on the system, the linear momentum of a system representing in
Eq. (4.13) is constant and the force representing in Eq. (4.14) should be zero. Figure 4.7
shows the constraints of center of mass and linear momentum of the system.
In free flight task, the initial conditions determine the linear momentum of the
system which is conserved.
30
r
.
ci
r
.
cj
r
.
ck
r
.
c
r
ci
r
cj
r
ck
r
c
Figure 4.7: Constraints of center of mass and linear momentum of the system
Angular Momentum Constraints
The angular momentum of a system is computed with respect to a fixpointand
the rotation related to this point. In free flight task, there is no external torque applied to
the system, thus the angular momentum of the system with respect to CM is constant and
therateofchangeiszero.Theseconstraintscanthenbeformulatedasfollows:
H
o
=
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
+ H
ci
=
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
+ J
i
ω
i
(4.15)
˙
H
o
=
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
H
ci
=
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
+ J
i
α
i
(4.16)
ρ
ci
= r
ci
− r
c
(4.17)
˙ ρ
ci
= ˙ r
ci
− ˙ r
c
(4.18)
¨ ρ
ci
=¨ r
ci
− ¨ r
c
(4.19)
31
where
H
o
− Angular momentum with respect to system CM.
H
ci
− Angular momentum of i
th
body with respect to i
th
body CM.
ρ
ci
− Relative CM position of i
th
body to system CM.
J
i
− Moment of inertial matrix of i
th
body represented in reference frame.
The vector representations above are all in reference frame. Figure 4.8 shows the
constraint related to i
th
body.
{I}
r
ci
ρ
ci
ω
i
{i}
r
c
H
o
α
i
H
o
.
Figure 4.8: Angular momentum constraint of the system
32
Chapter 5
Formulation of Human Body
Kinematics
5.1 Initial Guess from Measurements
Initial Guess of Orientation
Initial guess of orientation of each body in the system is generated by Wahba’s
problem given in Eq. (2.1) yielding a solution given in Eq. (2.13). For angular velocity and
acceleration of each body, a similar structure can be applied for estimation as follows.
Positions of marker p
1
and p
2
can be expressed as(see Fig. 5.1):
p
1
= r + Ra
1
(5.1)
p
2
= r + Ra
2
(5.2)
where r is of CM of the body, R is rotation matrix and a
1
and a
2
are location vectors with
respect to body CM represented in body frame.
33
{I}
r
a
1
p
1
p
2
a
2
Figure 5.1: Marker position estimated from CM location r
From Eqs. (5.1) and (5.2), we can have
˙ p
1
− ˙ p
2
=( ˙ r − ˙ r)+
˙
R( a
1
− a
2
) (5.3)
¨ p
1
− ¨ p
2
=(¨ r − ¨ r)+
¨
R( a
1
− a
2
) (5.4)
∆ ˙ p =
˙
R ∆ a=[[ ω]]· R ∆ a (5.5)
∆¨ p =([[ α]]+[[ω]][[ ω]])· R ∆ a (5.6)
In Eq. (5.5), if the orientation transformation R is known, then angular velocity
ω can be estimated using relative velocity between markers. Similarly in Eq. (5.6) angular
acceleration α can be estimated if ∆¨ p, R and ω are known.
Multiple pairs of relative velocities and accelerations can be measured and the
optimal ω and α can be estimated as follows:
min
ω
N
X
s=1
k ∆ ˙ p
s
−[[ ω]] R ∆ a
s
k
2
(5.7)
min
α
N
X
s=1
k ∆¨ p
s
−([[ α]]+[[ω]][[ ω]])· R ∆ a
s
k
2
(5.8)
34
where N is the number of vector measurements.
To find the extremum of the expression in Eq. (5.7), we have:
∂
∂ω
Ã
N
X
s=1
k ∆ ˙ p
s
−[[ ω]] R ∆ a
s
k
2
!
(5.9)
=
∂
∂ω
Ã
N
X
s=1
k ∆ ˙ p
s
+[[ R ∆ a
s
]] ωk
2
!
(5.10)
=
N
X
s=1
2[[ R ∆ a
s
]]
T
( ∆ ˙ p
s
+[[ R ∆ a
s
]] ω) (5.11)
Hence for local minimum, we have
N
X
s=1
[[ R ∆ a
s
]]
T
[[ R ∆ a
s
]] ω +
N
X
s=1
[[ R ∆ a
s
]]
T
( ∆ ˙ p
s
)=0 (5.12)
ω = SS
−1
·
Ã
N
X
s=1
[[ R ∆ a
s
]]
T
( ∆ ˙ p
s
)
!
(5.13)
where
SS =
N
X
s=1
[[ R ∆ a
s
]]
T
[[ R ∆ a
s
]]
=
N
X
s=1
k ∆ a
s
k
2
I
3
− R
Ã
N
X
s=1
∆ a
s
∆ a
T
s
!
R
T
(5.14)
Similarly for angular acceleration α
α = SS
−1
·
Ã
N
X
s=1
[[ R ∆ a
s
]]
T
( − ∆¨ p
s
+[[ ω]][[ ω]] R ∆ a
s
)
!
(5.15)
Matrix SS is invertible if two or more independent vector measurements are ob-
tained.
Initial Guess of Translation
Initial guess of CM location, velocity, and acceleration of a single body can also
be estimated after the orientation, angular velocity and acceleration are determined from
35
marker data. From Eqs. (5.1) and (5.2), we have
r = p
1
− Ra
1
(5.16)
r = p
2
− Ra
2
(5.17)
If rotation matrix R is already evaluated from marker locations, CM location rcanthenbe
estimated from each marker. The average CM location estimation from di fferent markers
is the initial guess of CM location of the segment. Initial guesses of CM velocity and accel-
eration can also be estimated using similar procedures via the first and second derivatives
of Eqs. (5.1) and (5.2).
36
5.2 Estimation Formulation
Estimation of Attitude
Consider a system of n interconnected rigid bodies with frames { B
i
} attached at
CM’s of rigid bodies as shown in Fig. 5.2.
{B }
1
{I}
{B }
2
x
y
z
x
y
z
x
y
z
x
y
z ......
{B }
3
{B }
n
Figure 5.2: Multi-body chain
The relative rotation between each frame and the reference frame is given by
rotation matrix R
i
o
. By writing Wahba’s problem for n rigid bodies, the optimization
problem can be written as the following:
min
R
1
··· R
n
n
X
i=1
N
X
s=1
X
i
s
k b
i
s
− R
i
o
a
i
s
k
2
(5.18)
subjected to
¡
R
i
o
¢
T
R
i
o
= I, i=1 ,··· ,n
where
n − number of rigid bodies
N − number of observations for each body.
X
i
s
− weighting coe fficient of s
th
measurement in i
th
body.
37
R
i
o
− a rotation matrix transforms a vector representation from i
th
frame to refer-
ence frame.
a
i
s
− s
th
measurement of i
th
body represented in i
th
body frame.
b
i
s
− s
th
measurement of i
th
body represented in reference frame.
TheproblemstatesinEq. (5.18)canbeviewedas nindependentWahbaproblems.
Here the superscripts of the vectors a
i
and b
i
indicate the body in which the measurements
are performed. The n separate Wahba’s problems are transformed to one optimization
problem by enforcing the interconnection between the adjacent bodies given in Eq. (4.5).
By penalty function method, the constraint can be incorporated into the objective function
as follows:
min
R
1
··· R
n
n
X
i=1
N
X
s=1
k b
i
s
− R
i
o
a
i
s
k
2
+ W
1
·
n
X
i, j=1,i6= j
°
°
r
ci
− r
cj
+ R
i
o
L
ij
− R
j
o
L
ji
°
°
2
(5.19)
In Eq. (5.19) the variables to be estimated are the rotation matrices R
i
o
,which
can be parameterized by quaternions via q-method. CM position r
ci
are assumed as known
values during the estimation process of attitudes. The only variables in Eq. (5.19) are R
i
o
.
W
1
is a sequence of weighting coe fficients and it represents the relative weighting between
the joint location error and the attitude estimation error.
Estimation of CM Position
Marker’s location measurements and the assumption of rigid body allows compu-
tation of CM location of each body. The constraints related to CM position estimation are
38
given in Eq. (4.5) and Eq. (4.12). Let r
∗
ci
denote the initial guess of the position of CM of
i
th
body from measurements. Using penalty function method to include the constraints in
estimation, the formulation is as follows:
min
r
ci
i=1,...n
n
X
i=1
k r
ci
− r
∗
ci
k
2
+ W
2
·
°
°
°
°
°
Mr
c
−
n
X
i=1
m
i
r
ci
°
°
°
°
°
2
+ W
3
·
n
X
i, j=1,i6= j
°
°
r
ci
− r
cj
+ R
i
o
L
ij
− R
j
o
L
ji
°
°
2
(5.20)
where W
2
and W
3
are sequences of weighting coe fficients related to CM location and joint
location constraints. r
ci
are the variables which need to be estimated in Eq. (5.20). As it
can be seen the joint location constraint is coupled in Eq. (5.19) and Eq. (5.20). As in Eq.
(5.19), when attitude is the variable to be estimated, r
ci
are assumed to be known values.
SimilarlyinEq.(5.20) R
i
,or q
i
, are viewed as known values in estimation of r
ci
.
The first summation in Eq. (5.20) can be interpreted as finding a solution r
ci
that is close to the initial guess r
∗
ci
from measurements. The relative weightings between
initial guess loss functions are set to be the equal, and so are the relative weighting between
various joint constraints.
Estimation of Angular Velocity
Initial guess of angular velocity ω
i
can be estimated from marker velocity mea-
surements of i
th
body in Eq. (5.13), or from the interpolation of quaternions. Constraints
related to angular velocity estimation are Eqs. (4.6) and (4.15). Similar to Eq. (5.20),
let ω
∗
i
be the initial guess of angular velocity of i
th
body, and angular velocity estimation
39
formulation including constraintsisasfollows:
min
ω
i
i=1 , ...n
n
X
i=1
k ω
i
− ω
∗
i
k
2
+ V
1
·
°
°
°
°
°
H
o
−
Ã
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
+ J
i
ω
i
!°
°
°
°
°
2
+ V
2
·
n
X
i,j=1,i6= j
°
°
° ˙ r
ci
− ˙ r
cj
+
˙
R
i
o
L
ij
−
˙
R
j
o
L
ji
°
°
°
2
(5.21)
where V
1
and V
2
are weighting coe fficient sequences relatedto angularmomentumandjoint
velocity respectively. The variables in Eq. (5.21) are ω
i
.
Estimation of CM Linear Velocity
Initial guess of CM linear velocity ˙ r
ci
of i
th
body can be calculated by r
ci
inter-
polation, or by marker velocity measurements. The constraints related to ˙ r
ci
estimation
are Eqs. (4.13)(4.15) and (4.6). Let ˙ r
∗
ci
be the initial guess of CM velocity of i
th
body,
the estimation formulation of CM linear velocity including constraints can be written as
follows:
min
˙ r
ci
i=1 , ...n
n
X
i=1
k ˙ r
ci
− ˙ r
∗
ci
k
2
+ V
3
·
°
°
°
°
°
M ˙ r
c
−
n
X
i=1
m
i
˙ r
ci
°
°
°
°
°
2
+ V
4
·
°
°
°
°
°
H
o
−
Ã
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
+ J
i
ω
i
!°
°
°
°
°
2
+ V
5
·
n
X
i,j=1,i6= j
°
°
° ˙ r
ci
− ˙ r
cj
+
˙
R
i
o
L
ij
−
˙
R
j
o
L
ji
°
°
°
2
(5.22)
where ˙ ρ
ci
= ˙ r
ci
− ˙ r
c
.
As it can be seen again, the angular momentum and joint velocity constraints are
coupled in Eqs. (5.21) and (5.22), but again, the variables in this estimation are only ˙ r
ci
.
40
V
3
, V
4
,and V
5
are sequences of weighting coe fficients related to linear momentum, angular
momentum and joint velocity respectively.
Estimation of Angular Acceleration
Initial guess of angular acceleration α
i
can be estimated from marker acceleration
measurements, or from the interpolation of angular velocity ω
i
. Constraints related to
angular acceleration estimation are Eqs. (4.7) and (4.16). Let α
∗
i
be the initial guess of
angular acceleration of i
th
body, it follows:
min
α
i
i=1,...n
n
X
i=1
k α
i
− α
∗
i
k
2
+ S
1
·
°
°
°
°
°
Ã
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
+ J
i
α
i
!°
°
°
°
°
2
+ S
2
·
n
X
i, j=1,i6= j
°
°
°¨ r
ci
− ¨ r
cj
+
¨
R
i
o
L
ij
−
¨
R
j
o
L
ji
°
°
°
2
(5.23)
where S
1
and S
2
are weighting coe fficient sequences related to external torques and joint
acceleration respectively.
Estimation of CM Linear Acceleration
Initial guess of CM linear acceleration ¨ r
ci
can be calculated by ˙ r
ci
interpolation,
or by marker acceleration measurements. The constraints related to ¨ r
ci
estimation are Eqs.
(4.14)(4.16) and (4.7). Let ¨ r
∗
ci
be the initial guess of acceleration, the estimation can be
41
written as follows:
min
¨ r
ci
i=1,...n
n
X
i=1
k¨ r
ci
− ¨ r
∗
ci
k
2
+ S
3
·
°
°
°
°
°
M¨ r
c
−
n
X
i=1
m
i
¨ r
ci
°
°
°
°
°
2
+ S
4
·
°
°
°
°
°
Ã
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
+ J
i
α
i
!°
°
°
°
°
2
+ S
5
·
n
X
i, j=1,i6= j
°
°
°¨ r
ci
− ¨ r
cj
+
¨
R
i
o
L
ij
−
¨
R
j
o
L
ji
°
°
°
2
(5.24)
The rate of change of angular momentum and joint acceleration constraints are
coupled in Eqs. (5.23) and (5.24), but again, the variables in this estimation are only ¨ r
ci
.
S
3
, S
4
,and S
5
are sequences of weighting coe fficients.
42
Chapter 6
Solutions of Kinematic Estimation
6.1 Computation of Orientation
In order to solve for the orientation of each body with joint position constraints,
the joint constraints in Eq. (5.19) are expressed in terms of quaternion using q-method as
follows:
min
R
1
··· R
n
n
X
i=1
N
X
s=1
k b
i
s
− R
i
o
a
i
s
k
2
+ W
1
·
n
X
i,j=1 ,i6= j
°
°
r
ci
− r
cj
+ R
i
o
L
ij
− R
j
o
L
ji
°
°
2
=min
R
1
··· R
n
n
X
i=1
N
X
s=1
°
°
b
i
s
°
°
2
+
°
°
a
i
s
°
°
2
−2( b
i
s
)
T
R
i
o
( a
i
s
)
+ W
1
·
n
X
i,j=1 ,i6= j
{k r
ci
− r
cj
k
2
+k L
ij
k
2
+k L
ji
k
2
+2( r
ci
− r
cj
)
T
R
i
o
L
ij
−2( r
ci
− r
cj
)
T
R
j
o
L
ji
−2 L
T
ij
¡
R
i
o
¢
T
R
j
o
L
ji
} (6.1)
43
The minimization problem in Eq. (6.1) is transformed into maximization as in
Eqs. (2.4-2.5). Define the objective function as
max
R
1
··· R
n
J
R
=max
R
1
··· R
n
n
X
i=1
N
X
s=1
( b
i
s
)
T
R
i
o
( a
i
s
)
+ W
1
·
n
X
i, j=1,i6= j
{ −( r
ci
− r
cj
)
T
R
i
o
L
ij
+( r
ci
− r
cj
)
T
R
j
o
L
ji
+ L
T
ij
¡
R
i
o
¢
T
R
j
o
L
ji
} (6.2)
Let R
k
o
represents the transformation from k
th
frame to reference frame denoted
as ” o”, parameterized by quaternion q
k
. Consider an open chain of rigid bodies without
branches. The terms related to k
th
body in expression of J
R
are evaluated as follows:
J
Rk
=
N
X
s=1
( b
k
s
)
T
R
k
o
( a
k
s
) (6.3)
+ W
1
·{( r
ck −1
− r
ck
)
T
R
k
o
L
k −1,k
+ L
T
k −1,k
³
R
k −1
o
´
T
R
k
o
L
k,k −1
} (6.4)
+ W
1
·{ −( r
ck
− r
ck+1
)
T
R
k
o
L
k,k+1
+ L
T
k,k+1
³
R
k
o
´
T
R
k+1
o
L
k+1 ,k
} (6.5)
Using the quaternion representation of the rotation transformation R
k
o
and the
relative rotation between i
th
and j
th
bodies written as q
j
i
= q
∗
i
⊗ q
j
,wehave:
g
k
( q)= q
T
k
( K
k
o
) q
k
(6.6)
+ W
1
{ q
T
k
( K
k
k −1
) q
k
+( q
∗
k −1
⊗ q
k
)
T
K
k −1,k
( q
∗
k −1
⊗ q
k
)} (6.7)
+ W
1
{ − q
T
k
( K
k
k+1
) q
k
+( q
∗
k
⊗ q
k+1
)
T
K
k,k+1
( q
∗
k
⊗ q
k+1
)} (6.8)
where, from Eqs. (2.5) and (2.9),
44
K
k
o
is calculated with b
k
s
and a
k
s
.
K
k
k −1
is calculated with ( r
ck −1
− r
ck
) and L
k −1 ,k
K
k
k+1
is calculated with ( r
ck
− r
ck+1
) and L
k,k+1
.
K
k −1,k
is calculated with L
k −1 ,k
and L
k,k −1
.
K
k,k+1
is calculated with L
k,k+1
and L
k+1 ,k
Note that k q
k
k
2
=1 is enforced, and q
∗
= q
−1
. Applying Lagrange multipliers as
in Eq. (2.11), Eqs. (6.6-6.8) can be written as
max
q
k
G
k
( q)
where q denotes the set of all quaternions relating to k
th
body, and
G
k
( q)= g
k
( q) − λ
k
q
T
k
q
k
(6.9)
Then we have:
1
2
∂G
k
∂q
k
= { K
k
o
+ W
1
·[ K
k
k −1
− K
k
k+1
+
©
q
∗
k −1
ª
T
L
K
k −1,k
©
q
∗
k −1
ª
L
+{ q
k+1
}
T
R
∗
K
k,k+1
{ q
k+1
}
R
∗
]}· q
k
− λ
k
q
k
(6.10)
where {·}
L
and {·}
R
∗
are defined in Eqs. (B.17) and (B.18). The condition for extremum
is then:
K
k
( q
k −1
,q
k+1
)· q
k
= λ
k
q
k
(6.11)
where
K
k
= K
k
o
+ W
1
[ K
k
k −1
− K
k
k+1
+
©
q
∗
k −1
ª
T
L
K
k −1 ,k
©
q
∗
k −1
ª
L
+{ q
k+1
}
T
R
∗
K
k,k+1
{ q
k+1
}
R
∗
] (6.12)
45
Assumingthat q
k −1
and q
k+1
areknown,Eq. (6.11)canbeviewedasaneigenvalue
problemthatyields q
k
. Asinq-method,theeigenvectorcorrespondstothelargesteigenvalue
of K
k
is the optimal quaternion to maximize G
k
.
If the k
th
body has more than two neighboring bodies, then for each joint, K
k
given in Eq. (6.12) has two additional terms. If the u
th
body is also connected to k
th
body,
then the additional terms on the right-hand side are
W
1
·
³
K
k
u
+{ q
∗
u
}
T
L
K
u,k
{ q
∗
u
}
L
´
(6.13)
or
W
1
·
³
− K
k
u
+{ q
u
}
T
R
∗
K
k,u
{ q
u
}
R
∗
´
(6.14)
For each body, solution of eigenvalue-eigenvector problem given in Eq. (6.11) will
be used to estimate its attitude. As it has been mentioned, the K
i
matrix related to i
th
body is a function of all the attitudes of bodies which have interconnection with i
th
body.
If initial guesses of attitudes of all bodies exist, then for certain weighting coe fficient in W
1
weighting sequence, a new set of attitudes of all bodies can be calculated.
6.2 Computation of CM Positions
Let J
CM p
be the objective function to be minimized in Eq. (5.20). Consider again
an open chain of rigid bodies, we have:
1
2
∂J
CM p
∂r
ck
=( r
ck
− r
∗
ck
)+ W
2
( − m
k
)
Ã
Mr
c
−
n
X
i=1
m
i
r
ci
!
+ W
3
( −1)
³
r
ck −1
− r
ck
+ R
k −1
o
L
k −1,k
− R
k
o
L
k,k −1
´
+ W
3
(1)
³
r
ck
− r
ck+1
+ R
k
o
L
k,k+1
− R
k+1
o
L
k+1,k
´
(6.15)
46
And yielding the following condition for extremum:
r
ck
+ W
2
· m
k
n
X
i=1
m
i
r
ci
+ W
3
·( − r
ck −1
+2 r
ck
− r
ck+1
)= Γ
k
(6.16)
where
Γ
k
= r
∗
ck
+ W
2
· m
k
Mr
c
+ W
3
·
³
R
k −1
o
L
k −1 ,k
− R
k
o
L
k,k −1
´
− W
3
·
³
R
k
o
L
k,k+1
− R
k+1
o
L
k+1,k
´
(6.17)
Or in matrix form:
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
W
2
m
k
m
1
I
3
.
.
.
( W
2
m
k
m
k −1
− W
3
) I
3
( W
2
m
2
k
+2 W
3
+1) I
3
( W
2
m
k
m
k+1
− W
3
) I
3
.
.
.
( W
2
m
k
m
n
) I
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
T
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
r
c1
.
.
.
r
ck −1
r
ck
r
ck+1
.
.
.
r
cn
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
0
3×1
.
.
.
0
3×1
Γ
k
0
3×1
.
.
.
0
3×1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(6.18)
By collecting terms, we have:
Λ
p
·[ r
ci,col v
]=[ Γ
i,col v
] (6.19)
where
[ r
ci,col v
]=
∙
r
T
c1
··· r
T
ck −1
r
T
ck
r
T
ck+1
··· r
T
cn
¸
T
(6.20)
[ Γ
i,col v
]=
∙
Γ
T
1
··· Γ
T
k −1
Γ
T
k
Γ
T
k+1
··· Γ
T
n
¸
T
(6.21)
47
Let
£
r
∗
ci,col v
¤
=
∙
r
∗ T
c1
··· r
∗ T
ck −1
r
∗ T
ck
r
∗ T
ck+1
··· r
∗ T
cn
¸
T
(6.22)
[ r
c,col v
]=
∙
r
T
c
··· r
T
c
··· r
T
c
¸
T
(6.23)
[ mI]=
∙
m
1
I
3
··· m
k −1
I
3
m
k
I
3
m
k+1
I
3
··· m
n
I
3
¸
(6.24)
[ m]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
m
1
I
3
0
3×3
··· ··· 0
3×3
0
3×3
.
.
.
.
.
.
.
.
. m
k
I
3
.
.
.
.
.
.
.
.
.
0
3×3
0
3×3
··· ··· 0
3×3
m
n
I
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(6.25)
where
h
r
∗
ci ,col v
i
is the matrix containing initial guesses of r
ci
column vectors aligned verti-
cally. Then,
Λ
p
= I
3 n
+ W
2
·[mI]
T
[mI]+ W
3
· C
rL,3 n
(6.26)
[ Γ
i,col v
]=
£
r
∗
ci,col v
¤
+ W
2
· M[ m][ r
c,col v
]+ W
3
· C
rR
(6.27)
Here I
3 n
is a 3 n dimensional identity matrix, and C
rL,3 n
is a 3 n×3 n matrix and
defined from C
rL
as follows:
C
rL,3 n
( x, y)= C
rL
( i, j)· I
3
,where i=1 ,··· ,n,and j=1 ,··· ,n.
x=(3 i −2) : (3 i) and y=(3 j −2) : (3 j).
C
rL
is a constant n× n matrix indicating the joint relationship between bodies.
Let C
rL
( i, j) denotes the element of i
th
row and j
th
column, then
C
rL
(i, i)= total number of bodies connecting to i
th
body.
C
rL
(i, j)= C
rL
( j, i)= −1,if i
th
and j
th
bodies are connected.
48
C
rL
(i, j)= C
rL
( j, i)=0,if i
th
and j
th
bodies are not connected.
For an open rigid body chain without branches,
C
rL
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
1 −10 ··· ··· 0
−12 −1
.
.
.
0 −12
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
−1
.
.
.
.
.
.
.
.
.
2 −1
00 ··· ··· −11
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(6.28)
In Eq. (6.27), C
rR
is a 3 n×1 matrix where k
th
subcolumn is given by:
C
rR
(3 k −2:3k,1) =
X
u
− R
k
o
L
k,u
+ R
u
o
L
u,k
(6.29)
where index u indicates which body connecting to k
th
body. In an open chain case, u =
( k −1) and ( k+1).
Giventheweightings W
2
and W
3
,thesolutions r
ci
canbeobtainedfromEq. (6.19)
as
[ r
ci,col v
]= Λ
−1
p
·[ Γ
i,col v
] (6.30)
6.3 Computation of Other Variables
For the computation of other variables, including angular velocity, CM velocity,
angularacceleration, and CMacceleration, thesolutionstructuresaresimilartoEq. (6.30).
The details of derivation are listed in appendix, Computation of Kinematic Estimation
section. The solutions for these variables are listed as follows:
[ ω
i,col v
]= Λ
−1
ω
·[ Ω
i,col v
] (6.31)
49
[ ˙ r
ci,col v
]= Λ
−1
v
·[
˙
Γ
i,col v
] (6.32)
[ α
i,col v
]= Λ
−1
α
·[
˙
Ω
i,col v
] (6.33)
[¨ r
ci,col v
]= Λ
−1
a
·[
¨
Γ
i,col v
] (6.34)
where the subscript col v denotes variable column vectors of all segments aligned vertically
as one column vector.
Eqs. (6.31 - 6.34) are the solutions for angular velocity, CM velocity, angular
acceleration,andCMacceleration,respectively. Intheequations, ontherighthandsidethe
matrices Λ
−1
x
contain the information of weighting coe fficients, and Ω,
˙
Γ,
˙
Ω,and
¨
Γ include
initial guesses and weightings. Given weightings and initial guesses, the new estimations
are obtained on the left hand side, thus iteration process is suggested to satisfy constraints.
50
6.4 Kinematic Computation Process
Self-iteration, Weighting iteration, and Stability
Details of Algo
p
for computation of orientation and CM location are shown in Fig.
6.1. For Algo
v
and Algo
a
, similar structure is applied. With certain weighting coe fficients,
self-iteration is defined as when inputting a set of variable values, a new set of values can
be calculated and iterated back to the estimation again until convergence. In Eq.(6.11),
K matrix is updated after a new set of quaternions was calculated. In Eqs (6.30), (6.31),
(6.32), (6.33) and (6.34), the input on the right-hand side of the equations can produce new
estimation on the left-hand side. After self-iteration two sets of variable values (in Algo
p
,
the two sets are quaternion and CM location of each body) are fed back to Algo
p
again
with increasing weightings. This outer iteration is defined as weighting iteration.
Figure 6.2 shows the concepts of self-iteration and weighting iteration for Eq.
(6.11). Theinitialguessofquaternionstartsfromcenterandorbitsaredefinedbyweighting
coe fficients. Self-iteration results converge to an orbit and jump to next orbit when the
weighting coe fficient increases. The iteration processes are repeated until reaching feasible
region where all constraints are satisfied.
51
Wahba's
Problem
Gap
+ W (k)
1
self-iteration
2
1
*
min ∑ −
=
n
i
ci ci
r r CM + W (k)
2
Gap
+ W (k)
3
self-iteration
q, r , k=1
ici
ic ic
q, r =r
ici ci
opt,k * ic
q, r
ici
opt,k opt,k
Weighting iteration, k=k+1
q, r
ici
opt opt
Figure 6.1: Iteration process in Algo
p
Feasible region
Nonfeasible region
Initial set
1 orbit
st
2 orbit
nd
Convergence path
Figure 6.2: Concept of self-iteration and weighting iteration of attitude estimation.
52
Nonfeasible
Region
m
1
m
2
m
3
Initial set
Feasible Region
Figure 6.3: Concept of self-iteration and weighting iteration of other variables.
FortheestimationsofCMlocation,velocityandacceleration,andangularvelocity
andaccelerationinEqs(6.30-6.34), Fig. 6.3showsdi fferentconvergencebehavior. Instead
of orbits, weighting coe fficients define approaching slopes to feasible region. The number
of self-iteration steps defines how far the estimation results stay on the same slope until
convergence in feasible region.
Theconvergencebehaviorsofvariableswithrespecttotheconstraintareconcluded
from simulation. Number of self-iteration steps controls how stable the solution stays on
an orbit or how long it stays on a path with fixed slope. The sequences of weighting
coe fficients determine the difference between orbits or slopes, and this a ffects the stability
and the location of convergence. The choice ofweighting coe fficients andself-iterationsteps
is di fferent for di fferent cases.
53
6.5 Interpolation of Kinematic Variables
In this work, besides the estimation from measurement mentioned in Sections 5.1
and ??, interpolation of variables is another means to obtain initial guess for estimation.
For example, in position estimation algorithm Algo
p
(see Fig. 3.1), quaternion of orien-
tation and CM position of each body are estimated, and in Algo
v
for velocity estimation,
initial guesses of angular and linear velocity are needed for evaluation. Initial guess of CM
velocity can be obtain from either interpolation of CM positions, or from marker velocity
measurements. However, initial guess of angular velocity from interpolation of orientation
needstosatisfytheunityconstraintofquaternion. Inordertopreservetheunityconstraint,
a three-angle parameterization of quaternion (orientation) is proposed. The concept of this
parameterization is from axis/angle representation of attitude.
q =
⎡
⎢
⎢
⎣
ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
= ⇒
⎡
⎢
⎢
⎣
ˆ n
θ
⎤
⎥
⎥
⎦
= ⇒
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
ϕ
1
ϕ
2
θ
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(6.35)
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
cos ϕ
2
sin ϕ
1
sin ϕ
2
cos ϕ
2
cos ϕ
1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(6.36)
Equation(6.35)showstheconceptofthree-angleparameterizationinFig. 6.4. Any
axis/anglerepresentationcanbetransformedintothree-anglerepresentationsincedirection
vector ˆ n is an unit vector and it only needs two angles, ϕ
1
and ϕ
2
, to describe the direction
as shown in Eq. 6.36. Instead of interpolating four elements of a quaternion directly, the
54
X
Y
Z
φ
1
φ
2
θ
n
^
n
x
n
y
n
z
Figure 6.4: Three-angle parameterization.
interpolation of ϕ
1
, ϕ
2
,and θ are performed, and thus the derivative of each angle can be
calculated to obtain angular velocity ω. The detail derivation of the relationship between
[ ˙ ϕ
1
˙ ϕ
2
˙
θ] and ω is in appendix Three-Angle Parameterization of Orientation section. Also,
the relationship between angular acceleration and [¨ ϕ
1
¨ ϕ
2
¨
θ] is evaluated.
Eqs (6.37) and (6.38) provide the estimation of first and second derivatives of
quaternion from angular velocity and acceleration.
˙ q = −
1
2
¯ ω ⊗ q (6.37)
¨ q = −
1
2
(¯ α ⊗ q+¯ ω ⊗ ˙ q) (6.38)
where
q = q
b
I
transforms vector representation from { b} to { I} frame.
ω =
¡
ω
b
I
¢
I
is the angular velocity of { b} relative to { I} frame expressed in { I}.
¯ ω=[ ω
T
,0]
T
.
55
α =
¡
α
b
I
¢
I
istheangularaccelerationof { b}relativeto { I}frameexpressedin { I}.
¯ α=[ α
T
,0]
T
.
56
Chapter 7
Dynamic Analysis of Human Body
7.1 Dynamic Equation Formulation
The dynamic formulation in this work is done by Lagrange equation formulation,
which is formulated as
d
dt
µ
∂T
∂ ˙ x
¶
−
∂T
∂x
+
∂V
∂x
= Q (7.1)
where
T is the kinetic energy and V is the potential energy of the system. x is a gen-
eralized coordinate that describes the system, and Q is the generalized force related to x.
The generalized coordinates in this work are a fixed position P of one of the bodies, and
quaternions q
i
ofrotationofallbodiesinthesystem. Theformulationofdynamicequations
canbesplitintotwoparts: translationandrotationparts. Theformulationofrotationpart
is referred and modified from [48].
57
X
Y
Z
{I}
P
L
e1
L
e2
C
1
C
2
r
1
r
2
L
en
r
n
......
{B }
1
{B }
2
{B }
n
......
C
n
Figure 7.1: Variable definitions in dynamic equation formulation.
Figure 7.1 shows the definition of variables used in dynamic formulation. L
ei
is
a vector connecting joints in i
th
body to ( i+1)
th
body expressed in i
th
body frame. r
i
is
CM location vector from one end joint in i
th
body expressed in i
th
body. C
i
represents the
positionof i
th
body CMlocation in reference frame. Let P bethe firstendpointlocationof
the first body and the total number of bodies in the system is n. R
i
o
is the rotation matrix
representing the rotation of i
th
body. Assuming simple-chain case is studied as shown in
Fig. 7.1, then
C
i
= P +
i −1
X
x=1
R
x
o
L
ex
+ R
i
o
r
i
(7.2)
If i=1,then
C
1
= P + R
1
o
r
1
(7.3)
From Eqs. (D.16) and (D.55), define
R
i
o
= G
i
L
T
i
(7.4)
58
,then
C
i
= P +
i −1
X
x=1
G
x
L
T
x
L
ex
+ G
i
L
T
i
r
i
(7.5)
and in matrix form, then
C
i
=
∙
I
3
G
1
+
L
e1
··· G
i −1
+
L
ei −1
G
i
+
r
i
0
3×4
··· 0
3×4
¸
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
P
q
1
.
.
.
q
i
.
.
.
q
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(7.6)
C
i
= E
i
· p (7.7)
where vectors
+
L
ex
and
+
r
i
are definedinEq. (D.47). p denotes the column vector of gener-
alized coordinates, including end location of first rigid body, P, and attitudes of i
th
body,
q
i
,and p is the variable used to formulate dynamic equations.
p =
∙
P
T
q
T
1
··· q
T
i
··· q
T
n
¸
T
(7.8)
and also, for CM velocity of i
th
body,
V
i
=
d
dt
C
i
=
˙
P +
i −1
X
x=1
2 G
x
˙
L
T
x
L
ex
+2 G
i
˙
L
T
i
r
i
=
˙
P +
i −1
X
x=1
2 G
x
+
L
ex
˙ q
x
+2 G
i
+
r
i
˙ q
i
(7.9)
V
i
is the linear velocity of CM of i
th
body, and it is the same as ˙ r
ci
in kinematic estimation.
Here another notation is used to prevent confusion with r
i
in this section. For Eq. (7.9), it
59
can be expressed in matrix-vector production form as follows:
V
i
=
∙
I
3
2 G
1
+
L
e1
··· 2 G
i −1
+
L
ei −1
2 G
i
+
r
i
0
3×4
··· 0
3×4
¸
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙
P
˙ q
1
.
.
.
˙ q
i
.
.
.
˙ q
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(7.10)
V
i
= K
i
· ˙ p (7.11)
If CM velocity can be expressed in the form of Eq. (7.11), then the translation kinetic
energy can be calculated as:
T
tr an s
=
n
X
i=1
1
2
m
i
V
T
i
V
i
(7.12)
=
n
X
i=1
1
2
m
i
˙ p
T
K
T
i
K
i
˙ p (7.13)
=
1
2
˙ p
T
Ã
n
X
i=1
m
i
K
T
i
K
i
!
˙ p (7.14)
=
1
2
˙ p
T
· M
t
· ˙ p (7.15)
where
M
t
=
n
X
i=1
m
i
K
T
i
K
i
(7.16)
For rotation kinetic energy
T
rot
=
n
X
i=1
1
2
¡
ω
i
I
¢
T
I
( J
i
)
I
¡
ω
i
I
¢
I
=
n
X
i=1
1
2
¡
ω
i
I
¢
T
i
( J
i
)
i
¡
ω
i
I
¢
i
(7.17)
As it can be seen in Eq. (7.17), angular velocity and moment of inertia of i
th
body can be
expressedineither reference frame { I} or body frame { b}. In this study,
1
2
¡
ω
i
I
¢
T
i
( J
i
)
i
¡
ω
i
I
¢
i
60
is used as rotation kinetic energy of i
th
body. From Eq. (D.77), Eq. (7.17) can be written
as
T
rot
=
n
X
i=1
1
2
( −2 L
i
˙ q
i
)
T
( J
i
)
i
( −2 L
i
˙ q
i
) (7.18)
=
n
X
i=1
2 ˙ q
T
i
L
T
i
( J
i
)
i
L
i
˙ q
i
(7.19)
Define J
∗
i
=4 L
T
i
( J
i
)
i
L
i
,then
T
rot
=
n
X
i=1
1
2
˙ q
T
i
J
∗
i
˙ q
i
(7.20)
Total kinetic energy is
T = T
tr a n s
+ T
rot
(7.21)
For Lagrange equation related to translation kinetic energy,
d
dt
µ
∂T
tr ans
∂ ˙ p
¶
= M
t
· ¨ p+
˙
M
t
· ˙ p (7.22)
where
˙
M
t
=
n
X
i=1
m
i
³
˙
K
T
i
K
i
+ K
T
i
˙
K
i
´
(7.23)
and
∂T
tr an s
∂p
=
1
2
C
1
· ˙ p (7.24)
where
C
1
( j)= ˙ p
T
·
∂M
t
∂p( j)
(7.25)
= ˙ p
T
·
n
X
t=1
m
i
µ
∂K
T
i
∂p( j)
K
i
+ K
T
i
∂K
i
∂p( j)
¶
(7.26)
and C
1
( j) is the j
th
row vector of C
1
matrix, and p( j) is the j
th
element of p vector,
j=1,...(3+4 n).
61
For rotation kinetic energy related to i
th
body, with Eqs (7.19) and (??), equation
related to q
i
can be calculated as
∂T
rot
∂ ˙ q
i
= J
∗
i
˙ q
i
and
d
dt
µ
∂T
rot
∂ ˙ q
i
¶
= J
∗
i
¨ q
i
+
˙
J
∗
i
˙ q
i
(7.27)
∂T
rot
∂q
i
=4
˙
L
T
i
( J
i
)
i
˙
L
i
q
i
(7.28)
d
dt
µ
∂T
rot
∂ ˙ q
i
¶
−
∂T
rot
∂q
i
= J
∗
i
¨ q
i
+
³
4
˙
L
T
i
( J
i
)
i
L
i
+4 L
T
i
( J
i
)
i
˙
L
i
´
· ˙ q
i
−4
˙
L
T
i
( J
i
)
i
˙
L
i
q
i
= J
∗
i
¨ q
i
+
³
4
˙
L
T
i
( J
i
)
i
L
i
+4 L
T
i
( J
i
)
i
˙
L
i
´
· ˙ q
i
+4
˙
L
T
i
( J
i
)
i
L
i
˙ q
i
= J
∗
i
¨ q
i
+8
˙
L
T
i
( J
i
)
i
L
i
˙ q
i
+4 L
T
i
( J
i
)
i
˙
L
i
˙ q
i
= J
∗
i
¨ q
i
+8
˙
L
T
i
( J
i
)
i
L
i
˙ q
i
(7.29)
In Eq. (7.29), relationships in Eqs. (D.27) and (D.29) are used. Define H
i
=4
˙
L
T
i
( J
i
)
i
L
i
,
and a Lagrange multiplier σ
i
, which corresponds to unity constraint of quaternion, Eq.
(7.29) can be written as
d
dt
µ
∂T
rot
∂ ˙ q
i
¶
−
∂T
rot
∂q
i
= J
∗
i
¨ q
i
+2 H
i
˙ q
i
+ σ
i
q
i
(7.30)
Although the unity constraint of quaternion is added to the equation related to
i
th
body, matrix J
∗
i
is still not full rank. In order to have a mass matrix that’s invertible,
the unity constraint is used again in a second order derivative form as in Eq. (D.42). If
external virtual torque related to i
th
body is τ
vi
,then
J
∗
i
¨ q
i
+2 H
i
˙ q
i
+ σ
i
q
i
= τ
vi
(7.31)
˙ q
T
˙ q + q
T
¨ q=0 (7.32)
62
In matrix form
⎡
⎢
⎢
⎣
J
∗
i
q
i
q
T
i
0
⎤
⎥
⎥
⎦
⎡
⎢
⎢
⎣
¨ q
i
σ
i
⎤
⎥
⎥
⎦
+
⎡
⎢
⎢
⎣
2 H
i
q
i
˙ q
T
i
˙ q
i
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
τ
vi
0
⎤
⎥
⎥
⎦
(7.33)
Eq. (7.33) can be formulated for each quaternion representing rotation of a body in the
system. If n bodies in a system is considered, then
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
J
∗
1
q
1
.
.
.
.
.
.
J
∗
n
q
n
q
T
1
0 ··· 0
.
.
.
.
.
.
.
.
.
.
.
.
q
T
n
0 ··· 0
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
¨ q
1
.
.
.
¨ q
n
σ
1
.
.
.
σ
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
+
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
2 H
1
q
1
.
.
.
2 H
n
q
n
˙ q
T
1
˙ q
1
.
.
.
˙ q
T
n
˙ q
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
τ
v1
.
.
.
τ
vn
0
.
.
.
0
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(7.34)
M
r
·
⎡
⎢
⎢
⎣
¨ q
σ
⎤
⎥
⎥
⎦
+C
2
=
⎡
⎢
⎢
⎣
τ
v
0
n×1
⎤
⎥
⎥
⎦
(7.35)
where
¨ q =
∙
¨ q
T
1
··· ¨ q
T
i
··· ¨ q
T
n
¸
T
(7.36)
C
2
=
∙
(2 H
1
q
1
)
T
··· (2 H
n
q
n
)
T
˙ q
T
1
˙ q
1
··· ˙ q
T
n
˙ q
n
¸
T
(7.37)
τ
v
=
∙
( τ
v1
)
T
( τ
v2
)
T
··· ( τ
vn
)
T
¸
T
(7.38)
In this matrix form, M
r
is invertible.
For potential energy V
V =
n
X
i=1
m
i
gh
i
(7.39)
where m
i
is the mass of i
th
body, g is the gravity acceleration value, and h
i
is the distance
relative to the level defined as 0 potential energy. Let ˆ jbe the unit vector in positive y axis
63
direction shown in Fig. 7.1 and it’s also the opposite direction of gravity. From Eq. (7.7),
partial derivative of potential energy can be calculated as
V =
n
X
i=1
m
i
g· ˆ j
T
C
i
(7.40)
=ˆ j
T
g·
n
X
i=1
m
i
E
i
p (7.41)
∂V
∂p( j)
=ˆ j
T
g·
n
X
i=1
m
i
µ
∂E
i
∂p( j)
p+ E
i
∂p
∂p( j)
¶
(7.42)
For the final dynamic equation formulation, from Eqs. (7.21), (7.22-7.26), (7.34-
7.35), and (7.42)
d
dt
µ
∂( T
tr ans
+ T
rot
)
∂ ˙ p( i)
¶
−
∂( T
tr an s
+ T
rot
)
∂p( i)
+
∂V
∂p( i)
= Q
i
(7.43)
M·
⎡
⎢
⎢
⎣
¨ p
σ
n×1
⎤
⎥
⎥
⎦
+C+G =Q (7.44)
where
M =
⎡
⎢
⎢
⎣
M
t
0
(3+4 n)× n
0
n×(3+4 n)
0
n× n
⎤
⎥
⎥
⎦
+
⎡
⎢
⎢
⎣
0
3×3
0
3×5 n
0
5 n×3
M
r
⎤
⎥
⎥
⎦
(7.45)
C =
⎡
⎢
⎢
⎣
³
˙
M
t
−
1
2
C
1
´
˙ p
0
n×1
⎤
⎥
⎥
⎦
+
⎡
⎢
⎢
⎣
0
3×1
C
2
⎤
⎥
⎥
⎦
(7.46)
G =
∙
G
T
0
1× n
¸
T
(7.47)
Q =
∙
( f
v
)
T
( τ
v
)
T
0
1× n
¸
T
(7.48)
where
G( j)=
∂V
∂p( j)
, f
v
=
∙
f
x
f
y
f
z
¸
T
, the force acting on end point of first rigid
body, and τ
v
=
∙
( τ
v1
)
T
( τ
v2
)
T
··· ( τ
vn
)
T
¸
T
.
64
X
Y
Z
{I}
X
P
r
f
Figure 7.2: Estimation of generalized force
Therelationshipbetweenvirtualandrealtorqueactingonthejointcanbederived.
InFig. 7.2,let X beapointonthebody,.and f istheforceactingonthispoint. Thelocation
of X can be expressed as
X = P + R· r (7.49)
and for virtual displacement,
δX = δP +
∂( Rr)
∂q
· δq (7.50)
From Eqs (D.51)(D.66)(D.69),
δX = δP +
³
2 G
+
r+2rq
T
´
· δq
= δP +
³
2 G
+
r
´
· δq
= δP +
³
2RL
+
r
´
· δq
= δP +2 R
¡
[[ r]] L+ rq
T
¢
· δq
= δP +2[[ r
I
]] RL· δq
= δP +2[[ r
I
]] G· δq (7.51)
65
For virtual work done by force f in Fig. 7.2
δW = f
T
· δX
= f
T
· δP +2 f
T
[[ r
I
]] G· δq
= f
T
· δP +2
³
[[ r
I
]]
T
f
´
T
G· δq
= f
T
· δP −2([[ r
I
]] f)
T
G· δq
= f
T
· δP −2( τ)
T
G· δq (7.52)
where r
I
is the representation of r in reference frame, and r is in body frame. r
I
= R
b
I
· r,
and [[ r
I
]] f = r
I
× f = τ is the torque in reference frame. Thus,
τ
T
vi
= −2( τ
i
)
T
G
i
(7.53)
Again, in Eq. (7.53), the sign di fference is due to the definition of quaternion angle. τ
i
is
the actual torque acting on i
th
body.
Using the formulation method proposed here, the interconnection constraints be-
tweensegmentsareinforcedandembededintheformulateionwithoutaddinganyLagrange
multipliersforconstraintforces. ThemodifiedNikraveshformulationrelatedtoquaternions
ensures the unity constraints of quaternions are satisfied. Then the dynamic formulation of
a multi-rigid-body system is complete at this stage.
66
Chapter 8
Algorithm Evaluation: Three Bar
Planer Linkage
8.1 Scheme Setup
ThreebarlinkagesetupinFig. 8.1simulateshumanjumpingmotion. Joint J
1
and
J
2
refer to knee and hip in the real human jumping experiment, and end point P represents
foot. The dynamic equations of motion of three bar linkage is governed by 5 parameters
also shown in Fig. 8.1. P(x, y)=[ P
x
,P
y
] are the end point XY coordinates, q
1
, q
2
,and q
3
are relative angles between neighboring bars or x axis. Marker locations on each segment
are shown as solid black dots in Fig. 8.2. A body frame is attached to each segment at CM
location, which is located at the center of each segment. The simulation is performed for
afree flight task with internal torques applied in Joint J
1
and J
2
,andthetimehistoryof
67
marker locations are recorded as the nominal set of estimation. Later marker locations are
perturbed with errors to simulate the data collected in real experiments.
X
Y
Z
{I}
P(x,y)
q
1
q
2
q
3
J
2
J
3
J
1
Figure 8.1: Scheme setup of three bar linkage system in 2D plane.
J
1
J
1
J
2
J
2
J
3
P
0.25m
0.5m
0.6m
0.3m
0.8m
0.5m
0.4m
0.25m
0.6m
0.4m
0.3m
0.2m
1m
Figure 8.2: Positions of markers of each segment.
68
The mass of each segment is 10 kilograms, and the torques applied in joint J
1
and
J
2
are both 0 N · m. The simulation time period is 1sec and the initial conditions are
P
x
=0( m); P
y
=0( m); q
1
= 45(deg); q
2
= 60(deg); q
3
= −75(deg);
˙
P
x
=2(m/s);
˙
P
y
=4( m/s); ˙ q
1
= 20(deg /s); ˙ q
2
= 50(deg /s); ˙ q
3
= −75(deg /s);
(8.1)
8.2 Constraint Matrix Normalization
The orientation estimation of k
th
body is determined from the eigenvector of K
k
matrix in Eq. (6.12), which can be written as follows:
K
k
= K
k
o
+ W
1
· K( q
k −1
,q
k+1
) (8.2)
In Eq. (8.2), the relative magnitudes of matrices K
k
o
and K would a ffect relative weighting.
Due to the choice of length unit or model scaling, K matrix might be huge at the beginning
compared to K
k
o
and weighting coe fficient W
1
might not be able to quantify the relative
weighting between original estimation from Wahba’s problem and the joint constraints,
which are related to K matrix in Eq. (8.2). The similar problem can be identified in the
estimation of other variables.
In order to eliminate the bias of relative magnitudes of matrices, normalization of
constraint matrices with respect to initial estimation matrices is proposed. The magnitude
of a matrix is defined as the largest singular value of the matrix. If the magnitudes of K
k
o
and K are
°
°
K
k
o
°
°
and k Kk, respectively, then by normalization, Eq. (8.2) is rewritten as
K
k
= K
k
o
+ W
1
·
Ã
°
°
K
k
o
°
°
k Kk
· K( q
k −1
,q
k+1
)
!
(8.3)
69
By normalization, all matrices in the same estimation process are of the same
magnitude, and the change in weighting coe fficients can quantify the relative e ffectiveness
of constraints.
8.3 Kinematic Analysis
Estimation of Position and Orientation
By changing the magnitude and sequence of weighting coe fficients, constraints are
tested one by one to evaluate the performance. The evaluation is started from orientation
andpositionestimationinAlgo
p
. Forjointconstraints,theerrorscomefromCMtranslation
errors and rotation errors. In this section, the e ffects of rotation and translation corrections
with respect to rotation and translation errors are discussed. Since the joint constraint is
coupled in position and orientation estimation equations, translation and rotation errors
are added as shown in Fig. 8.3 (b) and (c) to separate the e ffectofCMconstraintinCM
position estimation. That is, in this section, there is no error for CM location constraint,
andkMr
c
−
P
n
i=1
m
i
r
ci
k
2
=0. Thetimeframe,timestamp 0 .2second,ischosentoevaluate
the performance, and for other frames the same process are done. Number of self-iteration
steps is j
n
=15, and for each weighting coe fficient sequence, weighting iteration step index
is up to k
n
=10.
70
P
∆y
∆y
∆θ
∆θ
X
Y
Z
{I}
()
a
() b ()
c
J
1
J
2
Figure 8.3: Error types introduced in three bar linkage simulation.
Di fferent types and magnitudes of errors are tested for the performance and listed
below
∆ y=2 , 5 , 10 ( cm)
∆ θ=2 , 5 , 10 (deg)
Changing Weighting Sequence W
1
of Joint Constraint
In this section, W
1
is the only weighting sequence used to observe the convergence
behavior with respect to joint location constraint. The three sequences tested here are
W
1
( k
n
)= dx· x
kn −1
where dx=0 .01, k
n
=1 ,2 ,··· ,10,and x=[2 ,5 ,10] for sequence 1, 2, and 3.
For each weighting coe fficient, self-iteration results converge to fixed values and
form a stair shape as seen in Fig. 8.4. The errors introduced here are ∆ y =0 .1 m and
71
∆ θ=0deg. Thelargertheweightingcoe fficientis,thefastertheestimationresultconverges
toit’slimit. ForthetranslationerrorintroducedinFig. 8.4, rotationcorrectionaloneisnot
enough to close the gaps. Figure 8.5 shows the actual convergence behavior of the system
after each weighting iteration step of sequence 2. For rotation correction, the CM location
is fixed and the segment is rotated around it. Although the rotation correction finds the
attitudes that minimize the joint gaps, it is not good enough to eliminate joint location
discrepancy.
Figure 8.6 shows the behaviors of convergence due to di fferent values of initial
translation errors under rotation correction. When the initial error is large, the speed of
converge is relatively fast. The shape of the results in the figure suggest under W
1
=
dx · 2
kn −1
sequence of weighting coe fficient, the sequence needs to be longer to find the
extremum. Figure8.7showstherotationcorrectionbehaviorofjointgapwhenonlyrotation
error ∆ θ=10deg is present. As it can be expected, rotation correction fixes the error and
the joint gaps are zeros at the end of iteration for sequence 2 and 3. Figure 8.8 shows the
estimation of shapes of 3-bar linkage during weighting iteration process, and the joint gaps
are zeros. For di fferent rotation errors, Fig. 8.9 shows similar convergence behaviors as in
translationerrorcases. Butthedi fferenceisrotationcorrectioncaneliminaterotationerror
completely.
72
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
0.11
Iteration Steps (n)
Joint Gap 2 (m)
(b)
Seq. 1
Seq. 2
Seq. 3
Figure 8.4: Joint constraints with ∆ y=0 .1 m and ∆ θ=0deg
73
0.5 1 1.5
0.5
1
1.5
2
2.5
X (m)
Y (m)
Linkage Shape
0.52 0.54 0.56 0.58 0.6 0.62
2.26
2.28
2.3
2.32
2.34
2.36
X (m)
Y (m)
Joint gap 2
0.95 1 1.05
1.28
1.3
1.32
1.34
1.36
1.38
X (m)
Y (m)
Joint gap 1
Increasing n
Increasing n
Figure 8.5: Results of weighting iteration process. ∆ y=0 .1 m, W
1
= dx·5
kn −1
74
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
∆ y=0.02m
∆ y=0.05m
∆ y=0.1m
Figure 8.6: Joint constraint under weighting sequence W
1
= dx·2
kn −1
75
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
Seq. 1
Seq. 2
Seq. 3
Figure 8.7: Convergence under di fferent W
1
. ∆ θ=10 degrees.
76
0.4 0.6 0.8 1 1.2 1.4
0.8
1
1.2
1.4
1.6
1.8
2
2.2
2.4
2.6
X (m)
Y (m)
Linkage Shape
0.45 0.5 0.55
2.22
2.24
2.26
2.28
2.3
2.32
2.34
X (m)
Y (m)
Joint gap 2
1.02 1.04 1.06 1.08 1.1 1.12
1.32
1.34
1.36
1.38
1.4
1.42
1.44
X (m)
Y (m)
Joint gap 1
Increasing n
Increasing n
Figure 8.8: Gap convergence. ∆ θ=10 , W
1
= dx·5
kn −1
77
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
∆θ =2 deg
∆θ =5 deg
∆θ =10 deg
Figure 8.9: Gap convergence. W
1
= dx·5
kn −1
78
Changing Weighting Sequence W
2
of CM Constraint
Figure 8.10 shows the joint gap results due to CM location constraint correction.
Since CM location is not changed from nominal values, W
2
sequences don’t have e ffects on
joint gap elimination.
0 15 30 45 60 75 90 105 120 135 150
0.1
0.1
0.1
0.1
0.1
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0.1
0.1
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
Seq. 1
Seq. 2
Seq. 3
Figure 8.10: Di fferent W
2
convergences for joint gaps. ∆ y=0 .01 m
79
Changing Weighting Sequence W
3
of Joint Constraint
Figure 8.11 shows the translation correction with only W
3
sequencefortranslation
error. ∆ y =0 .1 m. Seq 1., 2. and 3. are definedasinprevioussections. Insteadof
staying in a certain orbit as the self-iteration of orientation estimation, self-iteration of CM
location estimation stays on a path with fixed slope towards constraint feasible region utill
changing weighting coe fficients. As a result seen in Fig. 8.11, the convergence behaviors
of joint gaps are also like the behaviors of CM location estimation. The number of self-
iteration determines how long the estimation result stays on the path with fixed slope.
In Figure. 8.11, the number of self-iteration is 15, and thus for every 15 iteration steps,
the weighting coe fficient changes resulting another slope of convergence path. Figure 8.12
shows the convergence shapes of weighting iterations. Similar convergence behavior can be
seen in Fig. 8.13 with di fferent initial translation errors, where the weighting sequence is
W
3
=0 .01×5
kn −1
, and the errors are ∆ y=0 .02 ,0 .05 ,and 0 .1 m
80
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
Seq. 1
Seq. 2
Seq. 3
Figure 8.11: Convergence with W
3
weighting sequence.
81
0.5 1 1.5
1
1.5
2
2.5
X (m)
Y (m)
Linkage Shape
0.45 0.5 0.55 0.6
2.25
2.3
2.35
X (m)
Y (m)
Joint gap 2
1 1.05 1.1
1.3
1.35
1.4
X (m)
Y (m)
Joint gap 1
increasing n
increasing n
Figure 8.12: Weighting results for gaps, W
3
=0 .01×2
kn −1
82
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 15 30 45 60 75 90 105 120 135 150
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
∆ y=0.02m
∆ y=0.05m
∆ y=0.1m
Figure 8.13: Convergence for di fferent ∆y.
83
Changing Weighting Sequences W
1
and W
3
of Joint Constraint
From results in previous sections, rotation correction and translation correction
have di fferent strengths when satisfying joint constraints. If only orientation errors are
present resulting in joint gaps, rotation correction is very e ffective, and so is translation
correction, since moving CM location of each body can always eliminate the joint gaps.
However, if only translation errors are present resulting in joint gaps, rotation correction
might encounter limitations when satisfying joint constraints. In this case. translation
correction can be used to satisfy joint constraints. Di fferent weightings will a ffect the ratio
between translation and rotation correction. Figure 8.14 gives the results of W
1
and W
3
weighting combination for errors ∆ y=0 .1 m and ∆ θ=10deg. If ratios between di fferent
typesoferrorsareknown,weightingcoe fficient sequences can be adjusted to have better
estimation results. A balanced choice of weighting factors tends to have better results
than if rotation or translation correction dominates the other. Here the rotation error
∆ θ =10deg creates a joint gap of about 0 .08 m, which is close to the gap created by
translation error ∆ y=0 .1 m. Figure 8.15 is the local magnification of Fig. 8.14, and it
showsthatthetranslationcorrectiondominatesovertherotationcorrection. Theknowledge
about errors suggests that the ratio of rotation and translation correction should be about
equal. Thus another set of weighting sequences is used and the results shows in Fig. 8.16.
The combination of W
1
=0 .01× 5
kn −1
and W
3
=0 .01× 2
kn −1
has more balanced results
for rotation and translation correction. Figure 8.17 shows the linkage shape estimations
with W
1
=0 .01×5
kn −1
and W
3
=0 .01×2
kn −1
.. In Fig. 8.18, nominal three bar shape at
84
time t=0 .2sec compares to di fferent sets of W
1
and W
3
. This is one of the guidelines of
weighting sequence choices.
85
0 30 60 90 120 150 180 210 240 270 300
0
0.05
0.1
0.15
0.2
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 30 60 90 120 150 180 210 240 270 300
0
0.05
0.1
0.15
0.2
Iteration Steps (n)
Joint Gap 2 (m)
(b)
W
1
=W
3
=0.01*2
k
W
1
=W
3
=0.01*5
k
Figure 8.14: Gap under di fferent W
1
and W
3
86
60 90 120
0.04
0.06
0.08
0.1
0.12
Iteration Steps (n)
Joint Gap 1 (m)
(a)
60 90 120
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Iteration Steps (n)
Joint Gap 2 (m)
(b)
W
1
=W
3
=0.01*2
k
W
1
=W
3
=0.01*5
k
Figure 8.15: Local area magnification in Fig. 8.14.
87
0 30 60 90 120 150 180 210 240 270 300
0
0.05
0.1
0.15
0.2
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 30 60 90 120 150 180 210 240 270 300
0
0.05
0.1
0.15
0.2
Iteration Steps (n)
Joint Gap 2 (m)
(b)
W
1
=0.01*5
k
,W
3
=0.01*2
k
W
1
=0.01*10
k
,W
3
=0.01*2
k
Figure 8.16: Proper weighting sequence combination
88
0.5 1 1.5
1
1.5
2
2.5
X (m)
Y (m)
Linkage Shape
0.3 0.4 0.5 0.6 0.7
2.1
2.2
2.3
2.4
2.5
X (m)
Y (m)
Joint gap 2
0.9 1 1.1 1.2 1.3
1.1
1.2
1.3
1.4
1.5
X (m)
Y (m)
Joint gap 1
increasing n
increasing n
Figure 8.17: Convergence for W
1
W
3
combination
89
0.5 1 1.5 2 2.5 3
0.5
1
1.5
2
2.5
X (m)
Y (m)
Linkage Shape
Nominal
W
1
=W
3
=0.01*2
k
W
1
=W
3
=0.01*5
k
W
1
=0.01*5
k
,W
3
=0.01*2
k
W
1
=0.01*10
k
,W
3
=0.01*2
k
Figure 8.18: Final shape results comparison.
90
Combination of Weighting Sequences W
1
, W
2
,and W
3
Variation
In this section, the errors are translation error in x direction, ∆ x=0 .1 m and
rotation error ∆ θ=10deg. This setup is intend to test CM constraint correction. Again
thetimeframe t =0 .2sec is the frame being studied. In Fig. 8.20, di fferent sets of
[ W
1
,W
2
,W
3
] are tested to evaluate the final shapes of three bar linkage. Weighting set
[ W
1
,W
2
,W
3
]=[ x
1
,x
2
,x
3
]means W
1
= dx× x
k
1
,W
2
= dx× x
k
2
,W
3
= dx× x
k
3
,and dx=0 .01.
In Fig. 8.21 the details of joints and end points are observed to determine the result of
whichsetofweightingisclosertonominal. Figure8.22givestheerroreliminatedratio with
di fferent values of W
3
. It can be seen that when weighting set [ W
1
,W
2
,W
3
]=[5 ,2 ,2] is
utilized,theerroreliminationismorebalanced. Figure8.23showsthesystem’sCMbehavior
under di fferent W
2
sequences. The magnitude of W
2
values only a ffects the convergence
speed, but not the location in this case(di fference is within 10
−6
m). Figure 8.24 plots the
final result of Algo
p
estimation. It can be seen the joint constraints and CM location of the
system are satisfied.
91
P
∆x
∆x
∆θ
∆θ
X
Y
Z
{I}
()
a
() b
Figure 8.19: Setup of errors: ∆ x=0 .1 m , ∆ θ=10deg
92
0.5 1 1.5 2 2.5
0.6
0.8
1
1.2
1.4
1.6
1.8
2
2.2
2.4
2.6
X (m)
Y (m)
Linkage Shape
nominal
[2, 2, 2]
[2, 2, 5]
[2, 2,10]
[5, 2, 2]
[5, 2, 5]
[5, 2,10]
[10, 2, 2]
[10,2, 5]
[10, 2,10]
Figure 8.20: Linkage shape comparison
93
1.35 1.4 1.45 1.5
2.6
2.65
2.7
2.75
X (m)
Y (m)
(a)
0.535 0.54 0.545 0.55 0.555
2.255
2.26
2.265
2.27
2.275
X (m)
Y (m)
(b)
0.3 0.35 0.4
0.6
0.65
0.7
0.75
X (m)
Y (m)
(c)
1.02 1.025 1.03 1.035 1.04
1.38
1.385
1.39
1.395
1.4
X (m)
Y (m)
(d)
Figure 8.21: Magnified joint locations and end points.
94
0 30 60 90 120 150 180 200
0
0.05
0.1
0.15
0.2
Iteration Steps (n)
Joint Gap 1 (m)
(a)
0 30 60 90 120 150 180 200
0
0.02
0.04
0.06
0.08
0.1
Iteration Steps (n)
Joint Gap 2 (m)
(b)
[5, 2, 2]
[5, 2, 5]
Figure 8.22: Gap correction under sets of weightings
95
0.82 0.83 0.84 0.85 0.86 0.87 0.88 0.89 0.9
1.7769
1.7769
1.7769
1.7769
1.7769
X (m)
Y (m)
[2, 2, 2]
[2, 5, 2]
[2,10,2]
moving direction
target CM location
starting point
Figure 8.23: Estimation of system’s CM location under di fferent W
2
sequences.
96
0 1 2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
X (m)
Y (m)
(a)
0 1 2
−1
0
1
2
3
X (m)
Y (m)
(b)
Est.
Nominal
Figure 8.24: Final estimation of the system.
Estimation of Linear and Angular Velocities
Figures8.26and8.27showtheresultsofconstraintsofangularmomentumcorrec-
tion. The figures show that for V
1
and V
4
weightings, the correction results are similar. In
Fig. 8.25, the concept of joint velocity correction by changing CM velocity and angular ve-
locityispresented. Forfixedlocationestimation,thelinearandangularvelocityestimations
can be found to satisfy joint velocity constraints. In Figs. 8.28 and 8.29, the convergence
resultsunder V
2
and V
5
weightingiterationsaresimilar. ForCMvelocityofthesystem,sim-
97
ilar convergence behaviors as in Fig 8.26 are observed. For acceleration estimation results,
the convergence behaviors with respect to the constraints are also similar.
P
v
1
v
2
r
.
c1
r
.
c2
ω
2
ω
1
Figure 8.25: The concept of joint velocity
0 15 30 45 60 75 90 105 120 135 150
0
0.1
0.2
0.3
0.4
0.5
0.6
iteration steps (n)
Angular momentum (kg−m
2
/s)
Ho difference compared to nominal
V1=0.01*2
k
V1=0.01*5
k
V1=0.01*10
k
Figure 8.26: Angular momentum di fference estimation, V
1
set.
98
0 15 30 45 60 75 90 105 120 135 150
0
0.1
0.2
0.3
0.4
0.5
0.6
iteration steps (n)
Angular momentum (kg−m
2
/s)
Ho difference compared to nominal
V4=0.01*2
k
V4=0.01*5
k
V4=0.01*10
k
Figure 8.27: Angular momentum di fference estimation, V
4
set.
99
0 15 30 45 60 75 90 105 120
0
0.001
0.002
0.003
0.004
0.005
0.006
0.007
0.008
0.009
0.01
iteration steps (n)
Gap velocity difference (m/s)
Gap1 velocity diffenence under V2 and V5 sequence
V2=0.01*2
k
V2=0.01*5
k
V2=0.01*10
k
V5=0.01*2
k
V5=0.01*5
k
V5=0.01*10
k
Figure 8.28: Joint 1 velocity di fference, V
2
and V
5
sets.
100
0 15 30 45 60 75 90 105 120
0
0.001
0.002
0.003
0.004
0.005
0.006
0.007
0.008
0.009
0.01
iteration steps (n)
Gap velocity difference (m/s)
Gap2 velocity diffenence under V2 and V5 sequence
V2=0.01*2
k
V2=0.01*5
k
V2=0.01*10
k
V5=0.01*2
k
V5=0.01*5
k
V5=0.01*10
k
Figure 8.29: Joint 2 velocity di fference, V
2
and V
5
sets.
101
Weighting Correction and Interpolation
Fromtheresultsinprevioussection, correctedvelocityestimationforasetoffixed
position estimation can be found to satisfy the velocity-related constraints However, the
velocity estimations might not be closed to the nominal sets. This discrepancy between
nominal reference and estimation results is illustrated in Fig. 8.30. If the variable x repre-
sents position estimation, and the initial guess is outside the feasible gray region of position
constraints, position estimation algorithm Algo
p
will find a set of position estimation that
satisfies all position-related constraints frame by frame inside the region. Since the initial
guess of velocity is based on interpolation of position estimation from Algo
p
,Fig. 8.30
shows that the estimated slope might be far from the reference. Due to the fact that for
each frame the errors are not uniform, the same weighting sequence will result convergences
with various distances to the reference. The same problem occurs when velocity estima-
tion is obtained in velocity estimation algorithm Algo
v
, and initial guess of acceleration is
evaluated from interpolation. This problem is illustrated in the next results section.
102
x
t
nominal reference
initial guess
estimation result
Figure 8.30: Relationship between nominal reference, inital guess, and estimation results.
Kinematic Final Results
After analyzing the constraint convergence behaviors due to di fferent types of
errors, thefinalkinematicanalysisisconductedbyperturbingthenominalmarkerlocations
with errors of normal distribution, which has 1 cm mean o ffset and standard deviation
σ =1 cm. Number of self-iteration steps is j
n
=15, and number of weighting iteration
steps is k
n
=10. Weighting coe fficient sequences are selected as follows
[ W
1
,W
2
,W
3
]= [5 ,2 ,2]
[ V
1
,V
2
,V
3
,V
4
,V
5
]= [2 ,2 ,5 ,5 ,5]
[ S
1
,S
2
,S
3
,S
4
,S
5
]= [2 ,2 ,5 ,5 ,5]
Seq uence( k+1) = dx× x
k
dx=0 .01 an d k=0 ,1 ,··· ,k
n
−1
103
Initial conditions of this test are the same as listed in Eq. 8.1. However, the forces
and torque at point P,andthetorquesatJoint J
1
and J
2
are listed as follows:
F
x
= F
y
=0 N
τ
p
= τ
1
=0 N · m
τ
J
1
= τ
2
=5 N · m
τ
J
2
= τ
3
=5 N · m
This setup simulates that in free flight phase, there are no external forces and torques
acting on the system. Only the internal torques between segments are present. Later on
the kinematic estimation results from Algo
p
,Algo
v
,andAlgo
a
will be the input of inverse
dynamic equation to estimate forces and torques of the system. There are five sets of
estimation that compare to the nominal reference. ic is the estimation with perturbed
nominal data, and sp
p
, sp
v
,and sp
a
denote the estimation sets using spline interpolation
of position, velocity and acceleration estimations from Algo
p
,Algo
v
,andAlgo
a
estimation
algorithms respectively. sp
pv a
is the estimation set which combines position estimation
fromAlgo
p
,velocityestimationfromAlgo
v
,andaccelerationestimationfromAlgo
a
without
interpolation.
In Figures 8.31-8.33, the angular position di fferences for each set of estimation is
less than 0 .03 rad(about 1 .72deg). Although for angle estimation, this is a satisfying result,
however, for angular velocity, the di fference is up to 4 rad/s (about 230deg /s). Since the
time interval for this simulation is 0 .01 sec,even 0 .01 rad di fference from the nominal set
in angle estimation, the velocity will have 1 rad/s di fference, and acceleration will have
100 rad/s
2
di fference from nominal estimation. Corresponding to the concept in Fig. 8.30,
104
as long as the initial guess is from interpolation, this phenomenon can not be avoided
completely.
Another phenomenon is that the algorithm correction can only find a set of esti-
mations near the initial guesses. In Figs 8.31-8.33, subplots (a)(c)(e) show the actual angle
values of each estimation set. In the plots (b)(d)(f) on the right-hand side, they show the
di fferences between the estimation set and nominal set values. It can be seen that although
the algorithm tries to converge the estimation to the nominal reference, the initial guess is
too far apart. Since the solutions that satisfy all constraints are not unique, an estimation
can be found around the initial guess neighborhood. Although the estimation of velocity
and acceleration is less than satisfaction, sp
pv a
set of estimation still satisfies all constraints
as seen in Figs. 8.34-8.37. The plots on the left-hand side in Figs. 8.34-8.37 are the same
plots on the right-hand side with di fferent scales. This is for clear performance illustration
of the algorithms.
105
0 0.5 1
0.2
0.4
0.6
0.8
1
time(sec)
angle(rad)
(a) q
1
0 0.5 1
0
0.01
0.02
0.03
0.04
time(sec)
angle(rad)
(b) q
1
diff
0 0.5 1
−4
−2
0
2
4
time(sec)
angular vel.(rad/s)
(c) dq
1
0 0.5 1
0
2
4
time(sec)
angular vel.(rad/s)
(d) dq
1
diff
0 0.5 1
−1000
0
1000
time(sec)
angular acc.(rad/s
2
)
(e) ddq
1
0 0.5 1
0
500
1000
time(sec)
angular acc.(rad/s
2
)
(f) ddq
1
diff
nominal ic
sp
p
sp
v
sp
a
Figure 8.31: Angle q
1
estimation.
106
0 0.5 1
1
1.5
2
2.5
time(sec)
angle(rad)
(a) q
2
0 0.5 1
0
0.02
0.04
time(sec)
angle(rad)
(b) q
2
diff
0 0.5 1
−2
0
2
4
time(sec)
angular vel.(rad/s
2
)
(c) dq
2
0 0.5 1
0
2
4
time(sec)
angular vel.(rad/s
2
)
(d) dq
2
diff
0 0.5 1
−1000
0
1000
time(sec)
angular acc.(rad/s
2
)
(e) ddq
2
0 0.5 1
0
1000
2000
time(sec)
angular acc.(rad/s
2
)
(f) ddq
2
diff
nominal ic
sp
p
sp
v
sp
a
Figure 8.32: Angle q
2
estimation.
107
0 0.5 1
−2
−1
0
1
time(sec)
angle(rad)
(a) q
3
0 0.5 1
0
0.01
0.02
0.03
0.04
time(sec)
angle(rad)
(b) q
3
diff
0 0.5 1
−4
−2
0
2
4
6
8
time(sec)
angular vel.(rad/s
2
)
(c) dq
3
0 0.5 1
0
2
4
time(sec)
angular vel.(rad/s
2
)
(d) dq
3
diff
0 0.5 1
−1000
0
1000
time(sec)
angular acc.(rad/s
2
)
(e) ddq
3
0 0.5 1
0
500
1000
1500
time(sec)
angular acc.(rad/s
2
)
(f) ddq
3
diff
nominal ic
sp
p
sp
v
sp
a
Figure 8.33: Angle q
3
estimation.
108
0 0.5 1
0
0.01
0.02
time(sec)
distance(m)
(a) Joint 1 diff.
0 0.5 1
0
1
2
3
4
time(sec)
joint velocity(m/s)
(c) dJoint 1 diff.
0 0.5 1
0
500
1000
1500
time(sec)
joint acc.(m/s
2
)
(e) ddJoint 1 diff.
0 0.5 1
0
1
2
3
x 10
−4
time(sec)
distance(m)
(b) Joint 1 diff.
0 0.5 1
0
0.02
0.04
time(sec)
joint velocity(m/s)
(d) dJoint 1 diff.
0 0.5 1
0
1
2
3
4
time(sec)
joint acc.(m/s
2
)
(f) ddJoint 1 diff.
ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.34: Joint 1 estimation
109
0 0.5 1
0
0.01
0.02
time(sec)
distance(m)
(a) Joint 2 diff.
0 0.5 1
0
1
2
3
time(sec)
joint velocity(m/s)
(c) dJoint 2 diff.
0 0.5 1
0
500
1000
1500
time(sec)
joint acc.(m/s
2
)
(e) ddJoint 2 diff.
0 0.5 1
0
2
4
x 10
−4
time(sec)
distance(m)
(b) Joint 2 diff.
0 0.5 1
0
0.02
0.04
time(sec)
joint velocity(m/s)
(d) dJoint 2 diff.
0 0.5 1
0
5
10
15
time(sec)
joint acc.(m/s
2
)
(f) ddJoint 2 diff.
ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.35: Joint 2 estimation
110
0 0.5 1
0
0.005
0.01
time(sec)
distance(m)
(a) CM diff.
0 0.5 1
0
0.5
1
1.5
time(sec)
CM velocity(m/s)
(c) dCM diff.
0 0.5 1
0
100
200
300
time(sec)
CM acc.(m/s
2
)
(e) ddCM diff.
0 0.5 1
0
0.5
x 10
−10
time(sec)
distance(m)
(b) CM diff.
0 0.5 1
0
1
2
x 10
−8
time(sec)
CM velocity(m/s)
(d) dCM diff.
0 0.5 1
0
0.5
1
x 10
−6
time(sec)
CM acc.(m/s
2
)
(f) ddCM diff.
ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.36: System CM.estimations
111
0 0.5 1
0
5
10
15
time(sec)
H(kg−m
2
/s)
(a) H diff.
0 0.5 1
0
1000
2000
3000
4000
5000
time(sec)
dH (kg−m
2
/s
2
)
(c) dH diff.
0 0.5 1
0
0.1
0.2
0.3
0.4
0.5
time(sec)
H(kg−m
2
/s)
(b) H diff.
0 0.5 1
0
1
2
3
4
5
6
time(sec)
dH(kg−m
2
/s&
2
)
(d) dH diff.
ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.37: Angular momentum estimation
112
8.4 Dynamic Analysis
Inverse Dynamics
Force and torque estimations are obtained by inverse dynamics. Figures 8.38-8.42
show the force and torque estimations from interpolated kinematics estimations. For τ
1
,
F
x
and F
y
, the estimated values from sp
pv a
set are close to nominal values. This is due
to the fact that the estimation of acceleration includes the constraints of external torques
and forces, which in a free flight task are zeros. Although the estimations of velocity
and acceleration themselves are not close to nominal estimations, zero external forces and
torquesaresatisfiedforfreeflightphasesimulation. However,asto τ
2
and τ
3
,theevaluated
torques are way o ff from nominal torques as seen in Figs. 8.39 and 8.40. The initial
guesses of velocity and acceleration are far from nominal estimation, and the estimation
algorithmisforcedtofindasetofestimationinthewrongneighborhood. Althoughdi fferent
interpolationtechniquesandfunctionscanbeappliedtoreduceestimationerrorsandnoises,
the estimation of velocity and acceleration can still be dissatisfactory.
Figures 8.43 and 8.44 show the torque estimations evaluated from direct mea-
surements of marker velocity and acceleration. The simulation perturbs nominal marker
velocity and acceleration and directly evaluates the initial guess of velocity and acceleration
via Eqs.(5.13) and (5.15) for velocity estimation algorithms Algo
v
and acceleration estima-
tion algorithm Algo
a
respectively. The torque estimations are now closer to the nominal
torques. Thisresultshowsthattheproperinitialguessofposition,velocityandacceleration
is crucial to the algorithm since solutions that satisfy all constraints are not unique.
113
0 0.2 0.4 0.6 0.8 1
−1
0
1
2
x 10
5
time(sec)
torque(N−m)
(a) torque 1
0 0.2 0.4 0.6 0.8 1
−100
0
100
time(sec)
torque(N−m)
(b)
0 0.2 0.4 0.6 0.8 1
−1
0
1
x 10
−4
time(sec)
torque(N−m)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.38: Torque τ
1
estimation results in di fferent scales
114
0 0.2 0.4 0.6 0.8 1
−1
0
1
2
x 10
5
time(sec)
torque(N−m)
(a) torque 2
0 0.2 0.4 0.6 0.8 1
−5000
0
5000
time(sec)
torque(N−m)
(b)
0 0.2 0.4 0.6 0.8 1
−1000
−500
0
500
1000
time(sec)
torque(N−m)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.39: Torque τ
2
estimation results in di fferent scales
115
0 0.2 0.4 0.6 0.8 1
−2
0
2
4
6
x 10
4
time(sec)
torque(N−m)
(a) torque 3
0 0.2 0.4 0.6 0.8 1
−500
0
500
1000
time(sec)
torque(N−m)
(b)
0 0.2 0.4 0.6 0.8 1
−200
0
200
400
time(sec)
torque(N−m)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.40: Torque τ
3
estimation results in di fferent scales
116
0 0.2 0.4 0.6 0.8 1
−10
−5
0
5
x 10
4
time(sec)
force(N)
(a) px force
0 0.2 0.4 0.6 0.8 1
−50
0
50
time(sec)
force(N)
(b)
0 0.2 0.4 0.6 0.8 1
−1
0
1
x 10
−4
time(sec)
force(N)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.41: Force p
x
estimation results in di fferent scales
117
0 0.2 0.4 0.6 0.8 1
−5
0
5
x 10
4
time(sec)
force(N)
(a) py force
0 0.2 0.4 0.6 0.8 1
−100
0
100
time(sec)
force(N)
(b)
0 0.2 0.4 0.6 0.8 1
−1
0
1
x 10
−5
time(sec)
force(N)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.42: Force p
y
estimation results in di fferent scales
118
0 0.2 0.4 0.6 0.8 1
−5
0
5
10
x 10
4
time(sec)
torque(N−m)
(a) torque 2
0 0.2 0.4 0.6 0.8 1
−50
0
50
time(sec)
torque(N−m)
(b)
0 0.2 0.4 0.6 0.8 1
2
4
6
8
time(sec)
torque(N−m)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.43: Torque τ
2
estimation results with velocity and acceleration measurements
119
0 0.2 0.4 0.6 0.8 1
−2
0
2
4
x 10
4
time(sec)
torque(N−m)
(a) torque 3
0 0.2 0.4 0.6 0.8 1
−50
0
50
time(sec)
torque(N−m)
(b)
0 0.2 0.4 0.6 0.8 1
2
4
6
8
time(sec)
torque(N−m)
(c)
nominal ic
sp
p
sp
v
sp
a
sp
pva
Figure 8.44: Torque τ
3
estimation results with velocity and acceleration measurements
120
Chapter 9
Experiment: 3D Human Jumping
Motion
9.1 Scheme Setup
Forexperimentaldataanalysis,Fig. 9.1showsthemarkerlocations,segments,and
joint locations. The hollow circles and black circles represent the locations of 75 markers
tracked in experiments. Hollow markers are in front of human body, and black ones are in
the back. 14 segments and 13 joints are modeled to represent human body. Segments are
modeled as rigid bodies, and joints are 3D rotational joints. The determination of ankle
and knee joint locations are shown in Fig. 9.2. For ankle, knee, wrist and elbow joints,
the midpoint of two lateral markers attached to the joint is the location estimation of that
joint. For shoulders and hips, Figs. 9.3 shows the estimation process. It is assumed that
theshoulderandhipjointlocationsareontheinterconnectionlineoflateralmarkersplaced
121
on shoulders and hips. After joint locatoins are determined, CM location of each segment,
except for trunk, is determined by longitudinal CM position percentage listed in Appendix
F,Table. F.1. ForCMoftrunk,thethicknessoftrunkisdeterminedbythedistancebetween
frontal and rear markers along z axis illustrated in Fig. 9.4. The longitudinal location is
also determined by longitudinal CM postion percentage, and the sagittal postion is in the
middleofthethickness. OncetheCMandjointlocationsofeachsegmentaredetermined, a
bodyframeisattachedtoeachsegmentatCMlocation,andmarkerlocationsinbodyframe
are evaluated. Later on in the experiment the marker locations are recorded in reference
frame, and by comparison of vectors in body and reference frames, the orientation of each
segment is calculated.
The detail dimensions of each segments are listed in Appendix F, and a free flight
task was performed for algorithm evaluation.
122
Upperarm L Upperarm R
Head M
Forearm L Forearm R
hand R hand L
Thigh R
Thigh L
Shank R Shank L
Foot R Foot L
Trunk M
ankle R
neck M
shoulder L shoulder R
wrist L
elbow R elbow L
wrist R
hip L
hip R
knee L
knee R
ankle L
X
Y
Figure 9.1: Experimental data collection setup
123
ankle L
knee L
X
Y
Z
CM
44.59%
lateral
markers
lateral
markers
Figure 9.2: Shank joint and CM locations determination
124
hips
6cm 6cm
10cm 10cm
left lateral
markers
right lateral
markers
shoulders
X
Y
Figure 9.3: Trunk joint and CM locations determination, frontal view
9.2 Experiment Setup
The setup of the experiment is illustrated in Fig. 9.5. The subject jumped from a
platform and the locations of markers on each segment were recorded by 7 cameras around
the subject. The duration of this free flight task was 0 .7 seconds, and the frame capture
frequency was 100 Hz. A body frame is attached at the CM location of each segment, and
the segment longitudinal direction is aligned with Y axis of the body frame. The time
history of xy z coordinates in reference frame of each marker was then analyzed to estimate
the orientation and CM location of each body segment. The CM of the whole human body
is estimated from CM locations of all segments. Since it’s a free flight task, the trajectory
of CM of the whole human body is calculated and fitted with a parabola as the nominal
CMconstraint. Similarly, for the angular momentum, it isconserved during freeflighttask.
The nominal angular momentum value is calculated by averaging the angular momentum
125
frontal
marker
rear
marker
midpoint of hip joints
interconnection
Z
Y
Neck
Trunk
length
43.40%
Figure 9.4: Trunk joint and CM locations determination, lateral view
values of all time frame. The nominal CM trajectory and system angular momentum are
as follows
CM
x
( t)= 1 .2356· t −1 .0512 (9.1)
CM
y
( t)= −4 .9· t
2
+2 .6068· t+1 .5992 (9.2)
CM
z
( t)= −0 .5644· t+0 .0819 (9.3)
H
sy s
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
1 .1969
−0 .0108
5 .2785
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(kg· m
2
/s) (9.4)
126
Y
Z
X
Figure 9.5: Human jumping experiment setup.
127
9.3 Weighting Setup and Dynamic Equation
From preliminary estimations of orientation and CM location, the experimental
data shows large discrepancy at shoulder and hip joints. This is due to the fact that the
shoulder and hip joint location estimations are not ideal. In order to have reasonable whole
body shape estimation, translation correction has more ratio in correcting joint gap errors
and other constraints Consequently, the weighting coe fficient sequences are selected as
follows:
[ W
1
,W
2
,W
3
]= [2 ,10 ,10]
[ V
1
,V
2
,V
3
,V
4
,V
5
]= [2 ,2 ,10 ,10 ,10]
[ S
1
,S
2
,S
3
,S
4
,S
5
]= [1 ,1 ,5 ,5 ,5]
Seq uence( k+1) = dx× x
k
dx=0 .01 an d k=0 ,1 ,··· ,k
n
−1
DynamicequationsofwholehumanbodyareformulatedwithgeneralizedcoordinatesofCM
location of trunk and quaternion of orientation of all segments. The formuation procedure
is the same in As a result, the net forces and torques related to trunk segment should be
zero.
9.4 Kinematic Estimation Results
Constraints Evaluation
In order to estimate orientation of a segment, three or more marker locations have
to be captured at each time instance. Due to the limitation of the experimental equipment,
128
only periods of first 0 .1sec and the last 0 .15sec meet the requirement of orientation estima-
tion. The following analysis results will thus split into two time periods. The constraints
of system integrity, CM constraints, and angular momentum of the system are evaluated
under the weighting coe fficient sequences in previous section. The number of self-iteration
is jn=15, and weighting iteration number is kn=10. In this section only few parts of the
constraints results are illustrated. For complete results, see appendix, Human Kinematic
Estimation Results section.
Figures. 9.6 - 9.8 show the results of discrepancies of ankle joint location, hip
joint velocity, and shoulder joint acceleration respectively. Estimation sets ic, sp
p
, sp
v
, sp
a
,
and sp
pv a
represent initial raw data estimation, spline of Algo
p
position estimation, spline
of Algo
v
velocity estimation, spline of Algo
a
acceleration estimation, and the combination
of Algo
p
,Algo
v
,andAlgo
a
estimation, respectively. For sp
pv a
estimation set, the errors of
jointposition,velocity,andaccelerationarelessthan10
−9
( m), 10
−6
(m/s),and 10
−3
(m/s
2
),
respectively.
129
0 0.05 0.1
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle L
0 0.05 0.1
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle R
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle L
0.55 0.6 0.65
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle R
ic
sp
p
sp
v
sp
a
sp
pva
Figure 9.6: Ankle joint location constraint evaluation
130
0 0.05 0.1
0
0.2
0.4
0.6
0.8
time(sec)
velocity diff.(m/s)
d hip L
0 0.05 0.1
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip R
0.55 0.6 0.65 0.7
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip L
0.55 0.6 0.65 0.7
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip R
ic
sp
p
sp
v
sp
a
sp
pva
Figure 9.7: Hip joint velocity constraint evaluation
131
0 0.05 0.1
0
1000
2000
3000
time(sec)
acc diff.(m/s
2
)
dd shoulder L
0 0.05 0.1
0
1000
2000
3000
4000
5000
time(sec)
acc diff.(m/s
2
)
dd shoulder R
0.55 0.6 0.65 0.7
0
100
200
300
400
500
time(sec)
acc diff.(m/s
2
)
dd shoulder L
0.55 0.6 0.65 0.7
0
100
200
300
400
500
time(sec)
acc diff.(m/s
2
)
dd shoulder R
ic
sp
p
sp
v
sp
a
sp
pva
Figure 9.8: Shoulder joint acceleration constraint evaluation.
Figures 9.9 and 9.10 show the convergence behaviors of di fferent estimation sets.
The performance of estimation set sp
pv a
is better than other sets with respect to CM and
angular momentum constraints. The error is less than 10
−4
oftheunitwehaveforeach
constraint.
132
0 0.05 0.1
0
0.005
0.01
time(sec)
displacement(m)
CM diff
0.55 0.6 0.65 0.7
0
0.005
0.01
time(sec)
displacement(m)
CM diff
0 0.05 0.1
0
0.2
0.4
time(sec)
velocity diff.(m/s)
d CM diff
0.55 0.6 0.65 0.7
0
0.2
0.4
time(sec)
velocity diff.(m/s)
d CM diff
0 0.05 0.1
0
5
10
time(sec)
acc diff.(m/s
2
)
dd CM diff
0.55 0.6 0.65 0.7
0
5
10
time(sec)
acc diff.(m/s
2
)
dd CM diff
ic
sp
p
sp
v
sp
a
sp
pva
Figure 9.9: System CM evaluation
133
0 0.05 0.1
0
1
2
3
4
time(sec)
H diff(kg−m
2
/s)
H diff
0.55 0.6 0.65 0.7
0
0.5
1
1.5
2
2.5
time(sec)
H diff(kg−m
2
/s)
H diff
0 0.05 0.1
0
1
2
3
4
5
x 10
4
time(sec)
dH diff(kg−m
2
/s
2
)
dH diff
0.55 0.6 0.65 0.7
0
500
1000
1500
time(sec)
dH diff(kg−m
2
/s
2
)
dH diff
ic
sp
p
sp
v
sp
a
sp
pva
Figure 9.10: Angular momentum and external torque evaluation
134
9.5 Dynamic Estimation Results
Inverse Dynamics
Forces and torques acting on the system can be evaluated using inverse dynamics.
Since the experiment was a free flight task,.the external forces acting on CM of trunk
(endpoint) should be zeros. As it can be seen in Fig. 9.11, the force estimations are close
to zeros. Figure. 9.12, the magnitudes of torques are in a reasonable range since during the
flight task, the motions of segments were smooth and slow. For all the forces and torques
estimation result, see appendix, Human Dynamic Estimation Result section.
135
0 0.02 0.04 0.06 0.08 0.1
−4
−2
0
2
x 10
−5
time(sec)
force (N)
trunk
0.55 0.6 0.65 0.7
−1.5
−1
−0.5
0
0.5
1
x 10
−6
time(sec)
force (N)
trunk
F
x
F
y
F
z
Figure 9.11: Forces acting on trunk
136
0 0.05 0.1
−2
−1
0
1
2
3
time(sec)
torque (N−m)
kneeL
0.55 0.6 0.65 0.7
−0.5
0
0.5
1
time(sec)
torque (N−m)
kneeL
0 0.05 0.1
−2
0
2
4
time(sec)
torque (N−m)
kneeR
0.55 0.6 0.65 0.7
−0.5
0
0.5
time(sec)
torque (N−m)
kneeR
τ
x
τ
y
τ
z
Figure 9.12: Torque acting on knee joint.
137
Forward Dynamics
Once the forces and torques are evaluated, forward dynamic can be preform to
evaluate segment orientations and CM locations comparing to raw marker data estimation.
Figures. 9.13 -9.15 showthebody shapecomparisonbetweenrawdataestimationset(dot-
tedline),algorithmkinematicestimationset(dashedline),andforwarddynamicestimation
set (solid line). As it can be seen in the figures, although algorithm estimation set satisfies
all joint constraint rather than raw data estimation set, there are discrepancies between
algorithm estimation and forward dynamic estimation.
One of the reasons is the original raw data does not have ideal estimation of joint
locations. The initial discrepancies of hip and shoulder joints are up to 10 cm.Thiswill
a ffect the performance of algorithm estimation since it is sensitive to initial guesses. The
other reason is improper initial conditions for each segment. If the initial conditions of
position, velocity and acceleration of each segment are o ff, the forward dynamic results will
diverge from algorithm estimation.
138
−1.4
−1
−0.4
1
1.5
2
−0.6
0
0.4
X
frame 1
Y
Z
Figure 9.13: Frame 1 in flight task.
139
−1
−0.5
1
1.5
2
−0.5
0
0.4
frame 3
Figure 9.14: Frame 3 in free flight task.
140
−1
−0.5
1
1.5
2 −0.6
0
0.4
frame 5
Y
X
Z
Figure 9.15: Frame 5 of free flight task.
141
Chapter 10
Summary of Works
In this study, the following tasks have been achieved:
• 1. Reformulation of Wahba’s problem for multi-body system
Original Wahba’s problem is formulated to estimate single rigid body attitude
in space using vector observation. In this study, reformulation of Wahba’s prob-
lem for multi-rigid-bodies is accomplished, and joint constraints are included in
the estimation. Original q-method solution for single body Wahba’s problem is
also modified to include joint constraints for multi-body system. The solution
estimation equation with constraints is also in the form of eigenvalue-eigenvector
problems.
2. Multi-body system formulation with constraints
The evaluation of kinematic system is formulated for estimations of rotation and
translation position, velocityandacceleration. Constraints relatedto the system
areincludedinthekinematicestimationformulation. Theconstraintsaresystem
142
integrity, including joint position, velocity, andaccelerationbetweenneighboring
bodies, system CM constraints, including CM positions, velocities, and accel-
erations, and system angular momentum constraints, including conservation of
angular momentum in free flight task and external torque evaluation.
3. Kinematic estimation of single rigid body
SimilartotheformulationofWahba’sproblemforestimationofsinglerigidbody
attitude, estimations of angular velocity and acceleration from marker velocity
and acceleration measurements are formulated and solved.
4. Kinematic estimation of multi-body constrained system
An iteration algorithm is created to search solutions around the neighborhood
of initial guess. Self-iteration and weighting iteration are defined to reach the
feasible region of constraints.
5. Interpolation of quaternion preserving unity constraint.
Three angle parameterization of orientation provides another alternative for ori-
entationrepresentation. Itiscloselyrelatedtoaxis/angleattituderepresentation
and does not have singularities for orientation estimation.
6. Dynamic equation formulation for rigid body chain with branches connected by
rotational joints
From Nikravesh’s derivation of dynamic equations using quaternions, a modi-
fied version of dynamic equation formulation is achieved. For rotational joint
constraints between bodies, the formulation procedure proposed in this work in-
cludes joint constraint in the formulation without adding Lagrange multipliers
143
as constraint forces, and at the same time, preserving the unity constraint of
quaternion.
7. Estimation of forces and torques in human body joints
For biomechanic applications, the estimation of forces and torques in body joint
provides possible understanding of muscle works. In doing so, motion injury for
the handicapped or senior people can be evaluated and avoided. For athletes,
the analysis of forces and torques in human joints might provides improvement
of performance.
144
Chapter 11
Future Work
11.1 Interpolation of Estimated Results
Although the algorithm proposed in this study suggests that the force estimations
from interpolation are less satisfied than the estimations from direct marker velocity and
acceleration measurements, di fferent types of interpolations can still be investigate to im-
prove estimation performance. With proper order of splines, the interpolation of positions
can reduce measurement noises and provide better velocity and acceleration estimations.
Moreover, in Algorithm Framework section, Fig. 3.1, the manipulations between di fferent
splineestimationsetscanalsohelptofindabetterneighborhoodofestimationconvergence.
If the raw data satisfies all constraints, spline sp
p
should be close to sp
v
.Inthissense,if
these two spline estimations are far apart, it implies the raw data might be too noisy.
145
11.2 Dynamic Weighting Sequences
For multi-body system discussed in this study, errors with respect to the same
constraint can be very di fferent. For example, in experimental data, the joint location
discrepancies ofshoulders and hipsareup to 10 cm, but for otherjoints asknees and ankles,
joint discrepancies are less than 2cm. In this research, it is proposed for all joint location
constraints, the weighting sequences are defined as the same. Di fferent weighting sequences
for the same constraint between di fferent segments might improve the estimation results if
moreknowledgeoftheerrorswithrespecttothesameconstraintisavailable. Theweighting
sequences can be adjusted corresponding to error di fferences dynamically.
11.3 Three Angle Parameterization for Orientation
For a body in 3D space, at least three parameters are needed to describe it’s
orientation. Although the formal proof has not been done for the application of three an-
gle parameterization in this work, minimum number of variables to describe orientation is
achieved without singularities. More investigation are needed for other orientation estima-
tion application and dynamic formulation with the three angles can be investigated.
11.4 Further Development of Algorithm
The three algorithms Algo
p
,Algo
v
,andAlgo
a
for estimation of positions, veloci-
ties, and accelerations can be rearranged of estimation sequence. The flow of the proposed
algorithm procedure is in a cascade style now and it can be arranged as parallel. The raw
146
data can be interpolated to estimate position, velocity, and acceleration as an estimation
set [ P
0
,V
0
,A
0
], and this set can be input to the three algorithms independently. If raw data
estimation is perfect, the algorithm estimation results from the three algorithms should be
similar,andthisagaincanbeinvestigatedtohaveabettersequenceofalgorithmestimation.
Other possible application is to integrate the algorithm into 3D motion capture
software. For example, Xsens Technologies B.V.[71] developed a motion capture suit that
incorporates three Analog Devices gyroscopic sensors, three accelerometers, three mag-
netometers and an Analog Devices Blackfin digital signal processor (DSP). The position
estimation is from integration of acceleration data captured by sensors. Incorporating with
the algorithm, the position integration error can be minimized, and better help the load
estimation in joints.
147
Bibliography
[1] J.K.AggarwalandQ.Cai.Humanmotionanalysis: Areview. Computer Vision and
Image Understanding, 73(3):428—440, March, 1999.
[2] I.Y. Bar-Itzhack. REQUEST: A Recursive QUEST Algorithm for Sequential Attitude
Determination. Journal of Guidance, Control, and Dynamics, 19(5):1034—1038, Sept.-
Oct. 1996.
[3] I.Y. Bar-Itzhack. New method for extraction the quaternion from a rotation matrix.
Journal of Guidance, Control, and Dynamics, 23(6):1085—1087, April 2000.
[4] I.Y. Bar-Itzhack, J. Deutschmann, and F.L.Markley. Quaternion normalization in
additive EKF for spacecraft attitude determination. pages 908—916, New Orleans, LA,
Aug 1991. Proceedings of the AIAA Guidance, Navigation, and Control Conference.
[5] I.Y. Bar-Itzhack and Y. Oshman. Attitude determination form vector observations:
Quaternionestimation. IEEETransactionson AerospaceandElectronicSystems,AES-
21(1):128—136, 1985.
[6] J. C. K. Chou. Quaternion kinematic and dynamic di fferential equations. Robotics and
Automation, IEEE Transaction on, 8(1):53—64, Feb 1992.
148
[7] James Coburn and Joseph J. Crisco. Interpolating three-dimensional kinematic data
using quaternion splines and hermite curves. Journal of Biomechanical Engineering,
127:311—317, 2005.
[8] C.E.Cohen. Global Positioning System: Theory and Applications,volume2.American
Institute of Aeronautics and Astronautics, Washington, 1996.
[9] P.B.Davenport. Avectorapproachtothealgebraofrotationswithapplications. 1968.
[10] Paolo de Leva. Adjustments to Zatsiorsky-Seluyanov’s segment inertia parameters.
Journal of Biomechanics, 29(9):1223—1230, 1996.
[11] K. Dobrovodsky. Quaternion position representation in robot kinematic structures.
volume1, pages561—564, Coventry, March1994.Inst.ofControlTheoryandRobotics,
Slovak Acad. of Sci, Control, 1994. control Volume 1., International Conference on.
[12] J. R. Dooley and J. M. McCarthy. Spatial rigid body dynamics using dual quaternion
components. volume1,pages90—95,Sacramento,CA,April1991.Dept.ofMech.Eng.,
CaliforniaUniv.,Irvine,CA,USA,RoboticsandAutomation,1991.Proceedings.,1991
IEEE International Conference on.
[13] J. R. Dooley and J. M. McCarthy. On the geometric analysis of optimum trajectories
for cooperating robots using dual quaternion coordinates. volume 1, pages 1031—1036,
Atlanta, GA, May 1993. Dept. of Mech. and Aerosp. Eng., California Univ., Irvine,
CA, USA, Robotics and Automation, 1993. Proceedings., 1993 IEEE International
Conference on.
149
[14] L. Euler. Nova methodus motum corporum rigidorum determinadi. Novi Commentari
Academiae Scientiarum Imperialis Petropolitanae, 20(7):189—207, 1775.
[15] D. Gebre-Eqziabher, G. H. Elkaim, J. D. Powell, and B. W. Parkinson. A gyro-free
quaternion-based attitude determination system suitable for implementation using low
costsensors. Position Location and Navigation Symposium, IEEE 2000,pages185—192,
March 2000.
[16] A. Glassner. Situation Normal Andrew Glassner’s Notebook- Recreational Computer
Graphics. Morgan Kaufmann Publishers, 1999.
[17] K. Halvorsen. A new method for estimating the axis of rotation and the center of
rotation. Journal of Biomechanics, 32(11):1221—1227, 1999.
[18] W. R. Hamilton. On quaternions, or an new system of imaginaries in algebra. Philo-
sophical Magazine, 25:489—495, 1844.
[19] A. J. Hanson. Constrained optimal framings of curves and surfaces using quaternion
Guass maps. Research Triangle Park, NC, Oct 1998. Dept. of Comput. Sci,. Indiana
Univ.,Bloomington, IN, USA, Visualization 98 Proceedings.
[20] A. J. Hanson and Hui Ma. Visualizing flow with quaternion frames. pages 108—115,
CP11, Washington, DC,Oct1994.Dept.ofComput.Sci., IndianaUniv.,Bloomington,
IN, USA, Visualization,1994., Visualization 94, Proceedings., IEEE Conference on.
[21] A. J. Hanson and Hui Ma. Quaternion frame approach to streamline visualization.
Visualization and Computer Graphics, IEEE Transaction on, 1(2):164—174, June1995.
150
[22] L. Herda, R. Urtasun, P. Fua, and A. Hanson. An automatic method for determining
quaternion field boundaries for ball-and-socket joint limits. pages 88—93, Washington,
DC, May 2002. Swiss Fed. Inst. of Technol., Lausanne, Switzerland, Automatic Face
and Gesture Recognition, 2002. Proceedings. Fifth IEEE International Conference on.
[23] Berthold K. P. Horn. Closed-form solution of absolute orientation using unit quater-
nions. Optical Society of America, 4:629—642, 1987.
[24] Berthold K. P. Horn, Hugh M. Hilden, and Shahriar Negahdaripour. Closed-form
solutionofabsoluteorientationusingorthonormalmatrices.OpticalSocietyofAmerica,
5(7):1127—1135, July, 1988.
[25] B.K.P. Horn. Robot Vision. MIT/McGraw-Hill, New York, 1986.
[26] Kenichi Kanatani. Analysis of 3D rotation fitting. Pattern analysis and machine
intelligence. IEEE Transactions on, 16(5):543—549, 1994.
[27] J. Keat. Analysis of least squares attitude determination routine DOAOP. CSC/TM-
77/6034, Lanhaam-Deabrock, MD, Feb 1977. Computer Sciences Corp.
[28] Myoung-Jun Kim and Myung-Soo Kim. A C
2
continuous B-spline quaternion curve
interpolating a given sequence of solid orientations. 1995.
[29] JeheeLeeandSungYounShin. Generalconstructionoftime-domainfiltersfororienta-
tion data. Visualization and computer graphics. IEEE Transactions on, 8(2):119—128,
2002.
151
[30] N.LovrenandJ.K.Pieper. Erroranalysisofdirectioncosinesandquaternionparame-
ters techniques for aircraft attitude determination. Aerospace and Electronic Systems,
IEEE Transactions on, 34(3):983—989, July 1998.
[31] J. L. Marins, Xiaoping Yun, E. R. Bachmann, R. B. McGhee, and M. J. Zyda. An ex-
tendedKalmanfilterforquaternion-basedorientationestimationusingMARGsensors.
volume 4, pages 2003—2011, Maui, HI, Oct 2001. Naval Postgraduate Sch., Monterey,
CA , USA, Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ Inter-
national Conference on.
[32] F.L.Markley. Attitudedeterminationusingvectorobservationsandthesingularvalue
decomposition. Journal of the Astronautical Sciences, 36(3):245—258, 1988.
[33] F. L. Markley. Attitude determination and parameter estimation using vector obser-
vations: Theory. Journal of the Astronautical Sciences, 37(1):41—58, 1989.
[34] F. L. Markley. Attitude determination and parameter estimation using vector obser-
vations: Application. Journal of the Astronautical Sciences, 39(3):367—381, 1991.
[35] F.L.Markley. Attitudedeterminationusingvectorobservations: Afastoptimalmatrix
algorithm. Journal of the Astronautical Sciences, 41(2):261—280, 1993.
[36] F. L. Markley. Fast quaternion attitude estimation from two vector measurements.
Journal of Guidance, Control, and Dynamics, 25(2):411—414, Oct 2001.
[37] F.L. Markley. A new quaternion estimation method. Journal of Guidance, Control,
and Dynamics, 17(2):407—409, 1994.
152
[38] F.L. Markley. How to estimate attitude from vector observations. American Astronau-
tical Society, AAS paper, pages 99—427, Aug 1999.
[39] F.L. Markley, E.J. Le fferts, and M.D. Shuster. Kalman filtering for spacecraft attitude
estimation. Journal of Guidance, Control, and Dynamics, 5(5):417—429, 1982.
[40] K. T. Miura, T. Nakaseko, and T. Ikedo. A new type of free-form curve given by a
integral form. pages 722—725, Hannover, June 1998. Shizuoka Univ., Japan, Computer
Graphics International, 1998. Proceedings.
[41] D. Mortari. Energy approach algorithm for attitude determination from vector obser-
vations. Journal of the Astronautical Sciences, 45(1):41—55, 1997.
[42] D. Mortari. ESOQ: A closed-form solution to the Wahba problem. Journal of the
Astronautical Sciences, 45(2):195—204, 1997.
[43] D. Mortari. Euler-q algorithm for attitude determination from vector observations.
Journal of Guidance, Control and Dynamics, 21(3):328—334, 1998.
[44] D. Mortari. ESOQ2: Single-point algorithm for fast optimal spacecraft attitude deter-
mination. Advances in the Astronautical Sciences, 95(2):817—826, 1999.
[45] D.Mortari. Secondestimatoroftheoptimalquaternion. Journal of Guidance, Control,
and Dynamics, 23(5):885—888, Oct 1999.
[46] D.Mrudin.Standbyattitudeestimation.PositionLocationandNavigationSymposium,
IEEE 2000, pages 429—436, March 2000.
153
[47] A. Nadler, I. Y. Bar-Itzhack, and H. Weiss. On algorithms for attitude estimation
using GPS. volume 4 of 3321-3326, Sydney, NSW, Dec 2000. Fac. of Aerosp. Eng.,
Technion-Israel Inst.of Technol., Haifa, Israe, Decision and Control, 2000. Proceedings
of the 39th IEEE Conference on.
[48] Parviz E. Nikravesh. Computer-aided Analysis of Mechanical Systems. Prentice-Hall,
Inc., New Jersey, 1988.
[49] F. C. Park and Bahram Ravani. Smooth invariant interpolation of rotations. ACM
Transactions on Graphics, 16(3):277—295, July, 1997.
[50] Mark L. Psiaki. Attitude determination filtering via extended quaternion estimation.
Journal of Guidance, Control, and Dynamics, 23(2):206—214, March 2000.
[51] M.L.Psiaki,F.Martel,andP.K.Pal. Three-axisattitudedeterminationviaKalmanfil-
tering of magnetometer date. Journal of Guidance, Control and Dynamics, 13(3):506—
514, 1990.
[52] K. S. Roberts. Coordinating a robot arm and multi-finger hand using the quaternion
representation. volume 2, pages 1252—1257. Dept. of Comput. Sci., Columbia Univ.,
New York, NY, USA, Robotics and Automation, 1990. Proceedings., 1990 IEEE Inter-
national Conference on, May 1990.
[53] A. M. Sabatini. Quaternion based attitude estimation algorithm applied to signals
from body-mounted gyroscopes. Electronics Letters, 40(10):584—586, May 2004.
[54] K. Shoemake. Animating rotation with quaternion curves. ACM SIGGRAPH, 1985.
154
[55] M. D. Shuster. A simple kalman filter and smoother for spacecraft attitude. Journal
of the Astronautical Sciences, 37(1):89—106, 1989.
[56] M. D. Shuster. In quest of better attitudes. pages 2089—2117, Santa Barbara, CA,
USA, Feb 2001. Proceeding of the 11th Annual AAS/AIAA Space Flight Mechanics
Meeting.
[57] Malcolm D. Shuster. A suboptimal algorithm for attitude determination form mul-
tiple star cameras. pages 383—393. Spaceflight mechanics 2000: Proceedings of the
AAS/AIAA Space Flight Mechanics Meeting, Jan 2000.
[58] M.D. Shuster. Kalman filtering of spacecraft attitude and the QUEST model. Journal
of the Astronautical Sciences, 38(3):377—393, Sept 1990.
[59] M.D. Shuster. A survey of attitude representations. Journal of Guidance, Control and
Dynamics, 41(4):439—517, 1993.
[60] M.D. Shuster and S.D. Oh. Three-Axis Attitude Determination from Vector Observa-
tions. Journal of Guidance and Control, 4(1):70—77, 1981.
[61] M.D. Shuster and S.D. Oh. Maximum likelihood estimate of spacecraft attitude. Jour-
nal of the Astronautical Sciences, 37(1):79—88, 1989.
[62] C.W.SpoorandF.E.Veldpaus.Rigidbodymotioncalculatedfromspatialcoordinates
of markers. Journal of Biomechanics, 13(4):391—393, 1980.
155
[63] A. J. Stoddart, P. Mrazek, D. Ewins, and D. Hynd. Marker based motion capture in
biomedical applications. Motion Analysis and Tracking, IEE Colloquium on, 103:4/1—
4/5, 1999.
[64] Qing Tan and J. G. Balchen. General quaternion transformation representation for
robotic application. volume 3, pages 319—324, Le Touquet, Oct 1993. Dept. of Eng.
Cybern., Norwegian Inst. of Technol., Trondheim, Norway, Systems. Man and Cyber-
netics, 1993. Systems Engineering in the Service of Human. Conference Proceedings.,
International Conference on.
[65] A.Ude. Nonlinearleastsquaresoptimizationofunitquaternionfunctionsforposeesti-
mation form corresponding features. Pattern Recognition, 1998. Proceedings, Fourteen
International Conference on, 1:425—427, Aug 1998.
[66] A. R. Vithani and K. C. Gupta. Estimation of object kinematics from point data.
Journal of Mechanical Design, 126(1):16—21, 2004.
[67] Vasily Volkov and Ling Li. Closed Form Solution for C
2
Orientation Interpolation.
Springer Netherlands, 2006.
[68] G. Wahba. A least squares estimate of satellite attitude. SIAM Review, 7(3):384—
386,409, 1965.
[69] J.R. Wertz. Spacecraft Attitude Determination and Control. D. Reidel Pub. Co.,
Boston, 1978.
156
[70] B. Xian, M. S. de Queiroz, D. Dawson, and I. Walker. Task-space tracking control of
robot manipulators via quaternion feedback. robotics and Automation, IEEE Transac-
tions on, 20(1):160—167, Feb 2004.
[71] Xsens. Movement science. http://www.xsens.com.
[72] Masanobu Yamamoto, Akitsugu Sato, and Satoshi Kawada. Incremental tracking of
human actions from multiple views. pages 2—7. Vision and Pattern Recognition, IEEE
Conference on, 1998.
[73] V. Zatsiorsky, V. Seluyanov, and L. Chugunova. In vivo body segment inertial para-
meters determination using a gamma-scanner method. Biomechanics of Human Move-
ment: Application in Rehabilitation, Sports and Ergonomics, pages 186—202, 1990.
[74] V. Zatsiorsky, V. Seluyanov, and L. Chugunova. Methods of determining mass-inertial
characteristics of human body segments. Contemporary Problems of Biomechanics,
pages 272—291, 1990.
[75] V. M Zatsiorsky, L. M. Raitsin, V. N. Seluyanov, A. S. Aruin, and B. J. Prilutzky. The
mass and inertia characteristics of the main segments of the human body. Journal of
Biomechanics, 8(B):1152—1159, 1983.
[76] V. M. Zatsiorsky and V. N. Seluyanov. The mass and inertia characteristics of the
main segments of the human body. Journal of Biomechanics, 8(B):1152—1159, 1983.
157
Appendix A
Rotation Matrix
A.1 Orthogonal Transformation
Let E = {b e
1
,b e
2
,b e
3
} and E
0
= {b e
0
1
,b e
0
2
,b e
0
3
} denote two orthonormal bases. Then an
arbitrary vector x can be represented either as
x = x
1
b e
1
+ x
2
b e
2
+ x
3
b e
3
or as x = x
0
1
b e
0
1
+ x
0
2
b e
0
2
+ x
0
3
b e
0
3
(A.1)
Thus, the representation vectors are
x ≡x
E
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
x
1
x
2
x
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
,and x
0
≡x
E
0 =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
x
0
1
x
0
2
x
0
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.2)
The bases, E and E
0
, can each be expanded in terms of the other basis as
b e
0
i
=
3
P
j=1
C
ij
b e
j
and b e
i
=
3
P
j=1
C
0
ij
b e
0
j
(A.3)
158
with
C
ij
=b e
0
i
·b e
j
and C
0
ij
=b e
i
·b e
0
j
(A.4)
Thus,
C
0
ij
= C
ji
or as matrices C
0
= C
T
(A.5)
The coe fficients C
ij
and C
0
ij
are the cosines of the angles between the two sets of axes, or
direction cosines,andthematrix C is called the direction-cosine matrix. From Eq. (A.4),
the columns of C
ij
are seen to be the representations of the b e
i
, i=1 ,2 ,3, with respect to
the basis E
0
while the rows of C
ij
are the representations of the b e
0
i
, i=1 ,2 ,3,withrespect
to the basis E. Thus, in terms of columns,
C=[(b e
1
)
E
0 ,(b e
2
)
E
0 ,(b e
3
)
E
0]=[(b e
0
1
)
E
,(b e
0
2
)
E
,(b e
0
3
)
E
]
T
(A.6)
From
x
i
=b e
i
· x ,and x
0
i
=b e
0
i
· x
(A.7)
it follows that
x
0
i
=
3
P
j=1
C
ij
x
j
,and x
i
=
3
P
j=1
C
ji
x
0
j
(A.8)
or, in matrix notation,
x
0
= Cx ,and x = C
T
x
(A.9)
where the indicated operation is matrix multiplication. Substituting these two equations
into one another, it follows immediately from Eq. (A.9) that
C
T
C = CC
T
= I ,or C
T
= C
−1
(A.10)
159
Such a matrix is said to be orthogonal. From Eq. (A.10)
1 = det(CC
T
) = (det C)(det C
T
) = (det C)
2
(A.11)
When det C =1, the orthogonal matrix is said to be proper or special, or denoted as
∈ SO(3), special othogonal matrices of dimension 3. Otherwise, it is improper. Proper
orthogonal matrices represent rotations. This is, if a matrix R is said to be special, it can
bearotationmatrixwhichcanbeusedtorotatevectorsorrepresentrotationtransformation
between two di fferent frames or coordinate systems.
A.2 Rotation Matrix
Arotationaboutthe zaxis,orrathertheb e
3
-axis,throughanangle θ isrepresented
pictorially in Fig A.1. Mathematically, this rotation is described by
θ ^
e
1
^
e'
1
^
e
2
^
e'
2
^
e
3
Figure A.1: Coordinate rotation
b e
0
1
=cos θb e
1
+sin θb e
2
, (A.12)
b e
0
2
= −sin θb e
1
+cos θb e
2
, (A.13)
b e
0
3
= b e
3
, (A.14)
160
or
b e
0
i
=
3
X
j=1
R
ij
(
b
3,θ)b e
j
(A.15)
Thus,
R(
b
3,θ)=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
cos θ sin θ 0
−sin θ cos θ 0
00 1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.16)
and
x
0
= R(
b
3,θ)x (A.17)
In this context the direction-cosine matrix R is called the rotation matrix. Necessarily,
R is proper orthogonal. Note that the rotation matrix is a function of the representation
of the axis of rotation and not of the abstract vector. (The value of the matrix changes if
the indices of the coordinate axes are altered, even if the physical rotation axis remains the
same.) Since the representation of the axis of rotation is necessarily the same with respect
to the primed and unprimed bases, no confusion can result as to which value to choose. For
rotations about the other two axes
R(
b
1,θ)
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
10 0
0cos θ sin θ
0 −sin θ cos θ
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
,R(
b
2,θ)=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
cos θ 0 −sin θ
01 0
sin θ 0cos θ
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.18)
obtained by cyclic permutation of equation (A.12) to equation (A.14).
161
A general expression for the rotation matrix for an arbitrary axis of rotation may
be obtained by noting that
R(
b
3,θ)
b
1=cos θ
b
1 −sin θ
b
2=cos θ
b
1 −sin θ
b
3×
b
1 (A.19)
R(
b
3,θ)
b
2=cos θ
b
2+sin θ
b
1=cos θ
b
2 −sin θ
b
3×
b
2 (A.20)
R(
b
3,θ)
b
3=
b
3 (A.21)
Hence, by analogy,if b n is an arbitrary unit column vector, and v
⊥
is the projection of the
column vector v, onto the plane perpendicular tob n (this is also a three-dimensional column
vector), then
R(b n, θ) v
⊥
=cos θv
⊥
−sin θb n× v
⊥
(A.22)
=cos θv
⊥
−sin θ[[ n]] v
⊥
(A.23)
Also, v
k
, the projection of v alongb n, is not changed by the rotation, since by analogy with
equation (A.21)
R(b n, θ)b n =b n (A.24)
Thus, in general,
R(b n, θ) v = v
k
+cos θv
⊥
−sin θ[[ n]]× v
⊥
(A.25)
and
v =(b n· v)b n −b n×(b n× v) (A.26)
= b nb n
T
v+[[b n]]
2
v (A.27)
= v
k
+ v
⊥
(A.28)
162
which gives the general decomposition of v into components which are parallel and perpen-
dicular to b n. Thus , for an arbitrary column vector v and arbitrary unit column vector
b n,
R(b n, θ) v =b nb n
T
v+cos θ[[b n]]
2
v −sin θ[[b n]] v (A.29)
Since v is an arbitrary column vector, it follows that
R(b n, θ)=cos θI+(1 −cos θ)b nb n
T
−sin θ[[b n]] (A.30)
or in terms of individual elements,
R(b n, θ)=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
c+ n
2
1
(1 − c) n
1
n
2
(1 − c)+ n
3
sn
1
n
3
(1 − c) − n
2
s
n
2
n
1
(1 − c) − n
3
sc+ n
2
2
(1 − c) n
2
n
3
(1 − c)+ n
1
s
n
3
n
1
(1 − c)+ n
2
sn
3
n
2
(1 − c) − n
1
sc+ n
2
3
(1 − c)
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.31)
where c=cos θ and s=sin θ,and b n=[ n
1
,n
2
,n
3
]
T
.
Using equation (A.31) to calculate Rv, the result can be interpreted as rotate
vector v through − θ about ˆ n as an axis, or as the new presentation of vector v in a new
coordinate frame which is θ angle more about ˆ n as an axis with respect to the original
frame. Any of the equations (A.29) through equation (A.31) is generally known as Euler’s
formula. It is clear that only four variables are needed to characterize R matrix. Three of
them are from b n, and the rest one is from θ, the angle. These four variables are related to
the quaternion, which will be defined in the following sections.
The following notation is used through out the whole paper: Arotation matrix
isdenotedby R
i
j
, R
i
j
∈ SO(3),thatdefinesatransformationofavectorrepresentationfrom
the i
th
frame to the j
th
frame. Also, a vector
− →
p is in space, and its representations in the
163
i
th
frame and j
th
frame are, respectively
(
− →
p )
in i
th
frame
=(
− →
p )
i
=
− →
a (A.32)
(
− →
p )
in j
th
frame
=(
− →
p )
j
=
− →
b (A.33)
Then the relation between these vector representations is
(
− →
p )
j
=
− →
b = R
i
j
− →
a = R
i
j
(
− →
p )
i
(A.34)
A.3 Derivative of Rotation Matrix
In Fig [ ], similar vector decomposition as Eqs. (A.12-A.14) can be done in 3D
space. In general case, rotation matrix R
b
I
can be expressed as
R
b
I
=
∙
¡
e
b
1
¢
I
¡
e
b
2
¢
I
¡
e
b
3
¢
I
¸
(A.35)
where
¡
e
b
i
¢
I
means the unit vector in i
th
orthogonal direction of body frame { b}, i=1,...3.
Clearlyrotationmatrix R
b
I
cantransformavectorinbodyframerepresentationtoreference
representation. If v
b
=[ x, y , z]
T
is the vector body frame representation, then the same
vector in reference frame will be
v
I
= R
b
I
v
b
=
∙
¡
e
b
1
¢
I
¡
e
b
2
¢
I
¡
e
b
3
¢
I
¸
·
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
x
y
z
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.36)
= x·
³
e
b
1
´
I
+ y·
³
e
b
1
´
I
+ z ·
³
e
b
1
´
I
(A.37)
In Eq. (A.37) since the unit bases of body frame is expressed in reference frame,
when multiplying with the vector representation in body frame, the equation can transform
the same vector into reference frame.
164
If Eq. (A.35) is di fferentiated with respect to time, then
˙
R
b
I
=
∙
¡
˙ e
b
1
¢
I
¡
˙ e
b
2
¢
I
¡
˙ e
b
3
¢
I
¸
(A.38)
=
∙
ω×
¡
e
b
1
¢
I
ω×
¡
e
b
2
¢
I
ω×
¡
e
b
3
¢
I
¸
(A.39)
=[[ ω]]
∙
¡
e
b
1
¢
I
¡
e
b
2
¢
I
¡
e
b
3
¢
I
¸
(A.40)
In Eq. (A.38), if a vector has a constant length, the time derivative of the vector
is the cross product with angular velocity of the vector as it can be seen in Eq. (A.39). In
Eq. (A.40), [[·]] is defined as cross product as:
[[ u]] v = u× v (A.41)
[[ u]] =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
0 − u
3
u
2
u
3
0 − u
1
− u
2
u
1
0
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(A.42)
Angular velocity ω in Eq. (A.39) is not well-defined. In order to make ω ×
¡
e
b
i
¢
I
meaningful, angular velocity vector ω has to be in reference representation. In this case,
angular velocity should be defined as
ω =
³
ω
b
I
´
I
(A.43)
ω
b
I
means when frame { I} is fixed, the rotation speed of frame { b} observed in frame { I}.
The outer subscription I indicates the vector is expressed in frame { I}. From the relative
rotation speed concept, it also implies that
ω
b
I
= − ω
I
b
(A.44)
Thus, Eq. (A.40) can then be written as
˙
R
b
I
=
hh³
ω
b
I
´
I
ii
R
b
I
(A.45)
165
If index b and I are reversed, then
˙
R
I
b
=
££¡
ω
I
b
¢
b
¤¤
R
I
b
(A.46)
= −
hh³
ω
b
I
´
b
ii
R
I
b
(A.47)
= −
hh
R
I
b
³
ω
b
I
´
I
ii
R
I
b
(A.48)
= − R
I
b
hh³
ω
b
I
´
I
ii
(A.49)
166
Appendix B
Quaternions
B.1 Quaternion Definition
Using i,j,and k to denote the Cartesian orthonormal basis for R
3
, any vector
v ∈ R
3
can be written in terms of this orthonormal basis, where
i =(1 ,0 ,0)
T
j =(0 ,1 ,0)
T
(B.1)
k =(0 ,0 ,1)
T
v=[ v
1
,v
2
,v
3
]
T
= v
1
i+ v
2
j+ v
3
k (B.2)
A quaternion may be regarded as a 4-tuple of real numbers, that is , as an element
of R
4
.Inthiscasewewrite
¯ q=[ q
1
,q
2
,q
3
,q
4
]
T
(B.3)
167
where q
1
, q
2
, q
3
,and q
4
are scalars. Let a scalar part of the quaternion be defined by q
4
,
and a vector part be given by q ∈ R
3
q =i q
1
+j q
2
+k q
3
(B.4)
wherei, j,and k are the standard orthonormal basis definedinequation(B.1). Thena
quaternion is given by:
¯ q =q+ q
4
=i q
1
+j q
2
+k q
3
+ q
4
(B.5)
The scalars q
1
, q
2
, q
3
,and q
4
are called the components of the quaternion. In a
matrix notation, a quaternion can also be written as
¯ q =
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(B.6)
B.2 Quaternion Properties
Further meaning to this definition by showing how quaternions are to be added
and multiplied must be given.
Equality of quaternions: If two quaternions are defined as the following,
¯ a =i a
1
+j a
2
+k a
3
+ a
4
and
¯
b =i b
1
+j b
2
+k b
3
+ b
4
(B.7)
¯ a =
⎡
⎢
⎢
⎣
a
a
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
1
a
2
a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
and
¯
b =
⎡
⎢
⎢
⎣
b
b
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
b
1
b
2
b
3
b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
168
then the two quaternion are equal if and only if they have exactly the same components.
That is, if
a
1
= b
1
a
2
= b
2
a
3
= b
3
a
4
= b
4
Addition:
The sum of the two quaternions a and b above is defined by adding the corre-
sponding components, that is
¯ a+
¯
b =i( a
1
+ b
1
)+j( a
2
+ b
2
)+k( a
3
+ b
3
)+( a
4
+ b
4
)
¯ a+
¯
b =
⎡
⎢
⎢
⎣
a+b
a
4
+ b
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
1
+ b
1
a
2
+ b
2
a
3
+ b
3
a
4
+ b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(B.8)
Product:
The quaternion product denoted by ⊗ is defined as the following:
¯ a ⊗
¯
b =
⎡
⎢
⎢
⎣
b
4
a+ a
4
b −a×b
a
4
b
4
−a·b
⎤
⎥
⎥
⎦
(B.9)
or in the form
¯ a ⊗
¯
b = b
4
a+ a
4
b −a×b+ a
4
b
4
−a·b (B.10)
169
Also,ifwehaveavector v ∈ R
3
,then
¯ a ⊗ v ⊗¯ a
∗
=(2 a
2
4
−|a|
2
) v+2(a· v)a −2 a
4
(a× v) (B.11)
B.3 Representation as Extension of Complex Numbers
Anotherimportantalgebraicconceptrelatingtoquaternions,aswellastoordinary
complexnumbers,willbeusefultousinwhatfollows,namely,thatofthecomplexconjugate
to a quaternion. we define the complex conjugate of the quaternion
¯ q =q+ q
4
=i q
1
+j q
2
+k q
3
+ q
4
(B.12)
to be the quaternion, denoted ¯ q
∗
,givenby
¯ q
∗
= −q+ q
4
= −i q
1
−j q
2
−k q
3
+ q
4
(B.13)
Norm:
The norm of a quaternion, denoted by N( q) or |¯ q|, is the scalar defined by
N( q)=
p
¯ q ⊗ ¯ q
∗
=
q
q
2
1
+ q
2
2
+ q
2
3
+ q
2
4
(B.14)
Inverse:
Using the ideas of the complex conjugate and the norm of a quaternion, we are
now able to show that every non-zero quaternion does have a multiplicative inverse, and we
can develop a formula for it. If we designate the inverse q
−1
,bydefinition of inverse, have
¯ q
−1
⊗ ¯ q =¯ q ⊗ ¯ q
−1
=1
¯ q
−1
=
¯ q
∗
|¯ q|
2
(B.15)
170
If |¯ q|=1,thenwehave
¯ q
−1
=¯ q
∗
(B.16)
B.4 Quaternion Product in Matrix Form
¯ a ⊗
¯
b can be written as matrix product form as the following:
¯ a ⊗
¯
b =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
4
a
3
− a
2
a
1
− a
3
a
4
a
1
a
2
a
2
− a
1
a
4
a
3
− a
1
− a
2
− a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
b
1
b
2
b
3
b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
b
4
− b
3
b
2
b
1
b
3
b
4
− b
1
b
2
− b
2
b
1
b
4
b
3
− b
1
− b
2
− b
3
b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
1
a
2
a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
= {¯ a}
L
(
¯
b)= {
¯
b}
R
(¯ a) (B.17)
Inthesameway,wecanderivetheproductof ¯ a
∗
⊗
¯
b,where ¯ a
∗
is the inverse of ¯ a, and unity
is assumed.
¯ a
∗
=
⎡
⎢
⎢
⎣
−a
a
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
− a
1
− a
2
− a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
¯ a
∗
⊗
¯
b =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
4
− a
3
a
2
− a
1
a
3
a
4
− a
1
− a
2
− a
2
a
1
a
4
− a
3
a
1
a
2
a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
b
1
b
2
b
3
b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
− b
4
b
3
− b
2
b
1
− b
3
− b
4
b
1
b
2
b
2
− b
1
− b
4
b
3
b
1
b
2
b
3
b
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
a
1
a
2
a
3
a
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
= {¯ a
∗
}
L
(
¯
b)= {
¯
b}
R
∗(¯ a) (B.18)
171
B.5 Quaternion and Rotation
For any normalized quaternion ¯ q,thatis, |¯ q|=1,wecanwrite
¯ q =q+ q
4
=ˆ nsin
θ
2
+cos
θ
2
(B.19)
or in vector form
¯ q =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
ˆ n
1
·sin
θ
2
ˆ n
2
·sin
θ
2
ˆ n
3
·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(B.20)
for some angle θ.Here ˆ n =
q
|q|
is a unit vector, which later can be shown as the same ˆ n
column vector in the equations (A.29) through equation (A.31). An arbitrary vector v can
be resolved into two orthogonal components: the component a along the vector q (or ˆ n)
and the component n normal to q.
v =a+n (B.21)
Thefollowingprocesswillprovethattheoperation ¯ q ⊗v ⊗¯ q
∗
isarotation,whichcorresponds
to the rotation matrix R. Since the vectora lies along the vectorq, a is simply some scalar
multiple of q,thatis
a = kq (B.22)
where k is some scalar. Then apply equation (B.11), we can have
¯ q ⊗a ⊗ ¯ q
∗
=¯ q ⊗ kq ⊗ ¯ q
∗
= kq (B.23)
172
Thisresultindicatesthatthecomponentaalongthevectorqisinvariantaftertheoperation.
For the normal component n, apply equation (B.11) again, we can have
¯ q ⊗n ⊗ ¯ q
∗
=( q
2
4
−|q|
2
)n+2(q·n)q −2 q
4
(q×n)
=( q
2
4
−|q|
2
)n −2 q
4
|q|(ˆ n×n) (B.24)
andhereweusethefactthat ˆ n =
q
|q|
.Let ˆ n×n =n
⊥
,andequation(B.24)canberewritten
as
¯ q ⊗n ⊗ ¯ q
∗
=( q
2
4
−|q|
2
)n −2 q
4
|q|n
⊥
) (B.25)
The following is the verification of the rotation of the normal component n after the oper-
ation.
Since n
⊥
and n are perpendicular to each other,
|n
⊥
| = |n× ˆ n| = |n||ˆ n|sin
π
2
= |n| (B.26)
. Inequation(B.20), q
4
=cos
θ
2
,and |q|=sin
θ
2
, and these can be substituted into equation
(B.25) resulting the following
¯ q ⊗n ⊗ ¯ q
∗
= (cos
2
θ
2
−sin
2
θ
2
)n −2cos
θ
2
sin
θ
2
n
⊥
=cos θ·n −sin θ·n
⊥
(B.27)
. The components of ¯ q ⊗n ⊗ ¯ q
∗
are illustrated in Figure B.1. The operation of ¯ q ⊗v ⊗ ¯ q
∗
can be shown as the following
¯ q ⊗v ⊗ ¯ q
∗
=¯ q ⊗(a+n) ⊗ ¯ q
∗
(B.28)
=¯ q ⊗a ⊗ ¯ q
∗
+¯ q ⊗n ⊗ ¯ q
∗
(B.29)
= a+m (B.30)
173
n
.
cos
-n
.
sin
n
n
θ
θ
Figure B.1: Rotation vector decomposition
where m=cos θ ·n − sin θ ·n
⊥
. From the verification process, we can prove that after
the operation ¯ q ⊗v ⊗ ¯ q
∗
, vector v has been rotated through − θ about q (or ˆ n)asanaxis.
Note that only for unit quaternions this rotation operation holds. Equation (A.22) through
equation (A.31) also represent the rotation process to derive the rotation matrix R.Ifwe
calculate Rv using the definition in equation (A.31), we will have the same result with
¯ q ⊗v ⊗ ¯ q
∗
.
B.6 Derivative of Quaternion
From Shuster’s survey of rotation, p482,483
d
dt
¯ η =
1
2
¯ ω ⊗¯ η (B.31)
where in the survey, ¯ η = q
I
b
,andinthedefinition of this work, Eq. (B.31) can be written as
˙ q
I
b
=
1
2
³
¯ ω
b
I
´
b
⊗ q
I
b
=
1
2
q
I
b
⊗
³
¯ ω
b
I
´
I
(B.32)
˙ q
b
I
=
−1
2
³
¯ ω
b
I
´
I
⊗ q
b
I
=
−1
2
q
b
I
⊗
³
¯ ω
b
I
´
b
(B.33)
where
¡
¯ ω
b
I
¢
b
and
¡
¯ ω
b
I
¢
I
are defined in Eqs. (A.43-A.44).
174
If the vector-angle representation of quaternion is defined as follows
q
b
I
=
⎡
⎢
⎢
⎣
ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
and q
I
b
=
⎡
⎢
⎢
⎣
−ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
(B.34)
,then
˙ q
b
I
=
⎡
⎢
⎢
⎢
⎢
⎣
˙
ˆ n·sin
θ
2
+ˆ n·cos
θ
2
Ã
˙
θ
2
!
−sin
θ
2
Ã
˙
θ
2
!
⎤
⎥
⎥
⎥
⎥
⎦
(B.35)
˙ q
I
b
=
⎡
⎢
⎢
⎢
⎢
⎣
−
˙
ˆ n·sin
θ
2
+ −ˆ n·cos
θ
2
Ã
˙
θ
2
!
−sin
θ
2
Ã
˙
θ
2
!
⎤
⎥
⎥
⎥
⎥
⎦
(B.36)
Comparing Eqs. (B.32), (B.35) and (B.33), (B.36), it follows
˙
θ = −ˆ n·
³
ω
b
I
´
b
= −ˆ n·
³
ω
b
I
´
I
(B.37)
sin
µ
θ
2
¶
˙
ˆ n =
1
2
∙
sin
µ
θ
2
¶
ˆ n×
³
ω
b
I
´
b
+cos
µ
θ
2
¶
ˆ n×
³
ˆ n×
³
ω
b
I
´
b
´
¸
(B.38)
= −
1
2
∙
sin
µ
θ
2
¶
ˆ n×
³
ω
b
I
´
I
−cos
µ
θ
2
¶
ˆ n×
³
ˆ n×
³
ω
b
I
´
I
´
¸
(B.39)
³
ω
b
I
´
b
= −
˙
θˆ n −sin θ
˙
ˆ n+(1 −cos θ)ˆ n×
˙
ˆ n (B.40)
³
ω
b
I
´
I
= −
˙
θˆ n −sin θ
˙
ˆ n −(1 −cos θ)ˆ n×
˙
ˆ n (B.41)
In Eqs. (B.40) and (B.41), if θ=2 nπ, n is an integer, then
³
ω
b
I
´
b
=
³
ω
b
I
´
I
= −
˙
θ· ˆ n (B.42)
That means
¡
ω
b
I
¢
b
k ˆ n,and
˙
ˆ n =
− →
0.
175
The sign di fference in Eqs. (B.40) and (B.41) between Shuster and this work is
because the definition of the angle in quaternion is di fferent. In this work, when using
expression such as
q
b
I
=
⎡
⎢
⎢
⎣
ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
, it means the angle is definedbydirectionshowninFig. []. InFig. [],sincequaternion q
b
I
is used to transform a vector from frame { b} to frame { I}, it is intuitive to define the angle
di fference from frame { b} to frame { I}, not from frame { I} to frame { b} as in Shuster’s
survey. Once the direction of direction unit vector is defined, by the right-hand rule, the
sign of angle can be determined as shown in Fig. [ ]. That is, when frame { b} rotates along
z axis with a positive angle rate
˙
θ
z
, from body-frame point of view, frame { I} is rotating
with a negative angle rate −
˙
θ
z
. Thus, in the example in Fig. [], using Eq. (B.42) follows
³
ω
b
I
´
b
=
³
ω
b
I
´
I
= −( −
˙
θ
z
)ˆ n =
˙
θ
z
ˆ n (B.43)
, which corresponds to the result in the survey.
176
Appendix C
Three-Angle Parameterization of
Orientation
C.1 Definition of Three Angles
Figure C.1 shows the three-angle parameterization. The concept of this parame-
terization is that any quaternion which represents a rotation can be viewed as two parts:
an unit vector and an angle. In Eqs. (B.19) and (B.20) this idea is represented. Since a
unit vector is confined in a unit sphere, only two angles are needed to define a unit vector
as seen in Fig. C.1. Let ϕ
1
and ϕ
2
be the two angles which define the direction of unit
vector, and θ is the angle rotates along the unit vector as it has been defined in quaternion.
177
The flow chart below shows the procedure of the parameterization.
q =
⎡
⎢
⎢
⎣
ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
Step 1
= ⇒
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
n
x
n
y
n
z
θ
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
Step 2
= ⇒
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
ϕ
1
ϕ
2
θ
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
where n
x
, n
y
,and n
z
aretheelementsofvector ˆ n. Step1transformaquaterniontoavector-
angle representation, and step 2 transform the vector-angle representation to three-angle
representation, which is illustrated in Fig. 2.1.where
X
Y
Z
φ
1
φ
2
θ
n
^
n
x
n
y
n
z
Figure C.1: Three-angle parameterization.
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
cos ϕ
2
sin ϕ
1
sin ϕ
2
cos ϕ
2
cos ϕ
1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.1)
Some notes need to be emphasized before step 1. Quaternion q presents the same
rotation as − q. That means for consecutive time frames, the estimations of quaternions
might be inconsistent from frame to frame since quaternions are solved from q-method,
178
which is an eigenvalue-eigenvector solution, and q and − q are the same eigenvector with
di fferent coe fficients. For this reason, the estimated quaternion in one time frame needs to
bealignedtobeclosetothequaternionestimatedintheprevioustimeframe. Inordertodo
so, quaternion in firsttimeframeneedstobedefined first as q( t
1
), and then for estimation
of quaternion q( t
2
) in next time frame, ± q( t
2
) are tested for inner product to verify which
direction is close to q( t
1
). The inner product of q( t
1
) and q( t
2
) with the direction wished to
be chosen should be positive. Once q( t
2
) is determined, ± q( t
3
) can be aligned with respect
to q( t
2
) and so on.
Similar alignment problem occurs for vector-angle representation of a quaternion.
As it can be seen in Eq. (C.2), for a quaternion, there are two sets of unit vectors and
angles that correspond to the same quaternion.
q =
⎡
⎢
⎢
⎣
ˆ n·sin
θ
2
cos
θ
2
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
−ˆ n·sin
− θ
2
cos
− θ
2
⎤
⎥
⎥
⎦
(C.2)
C.2 Estimation of Three Angles
Because of non-unique vector-angle representation of a quaternion, in step 1 the
unit vector ˆ n of each time frame needs to be aligned with the one in previous time frame.
Since quaternions are aligned for every time frame before step 1, once direction unit vectors
are aligned, the angles in adjacent time frame should be close to each other, too. The
alignment procedures of direction unit vectors are as follows:
Step 1 procedures:
1. For quaternion q( t
1
) at time t
1
, assign ˆ n
t
1
and θ
t
1
as the direction unit vector
and angle.
179
2. For q( t
2
) defined as
q
t
2
=
⎡
⎢
⎢
⎣
q
t
2
q
t
2
,4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
t
2
,1
q
t
2
,2
q
t
2
,3
q
t
2
,4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
ˆ n
t
2
·sin
θ
t
2
2
cos
θ
t
2
2
⎤
⎥
⎥
⎦
(C.3)
, calculate kq
t
2
k. In Eq. (C.3), this value equals to
°
°
°sin
θ t
2
2
°
°
° since ˆ n
t
2
is a unit vector.
3. Let
ˆ n
t
2
=
q
t
2
kq
t
2
k
(C.4)
and then calculate the inner products of ˆ n
t
1
and ±ˆ n
t
2
to determine which direction of ˆ n
t
2
is close to ˆ n
t
1
.
4. Once the direction of ˆ n
t
2
is determined, θ
t
2
can be determined from Eq. (C.3).
5. Repeat the procedures above for q( t
3
) to align with q( t
2
), and so on for the rest
of quaternions.
In procedure 4, the estimation range of θ is between 0 to 2 π.Thiswillcause
inconsistencyofanglevaluesbetweenframes. Forexample,ifattime t
i
, θ
i
=2
◦
,andattime
t
i+1
, θ
i+1
= 359
◦
, since it is unlikely to havesuch abig angledi fference betweenconsecutive
frames, then it is most likely that θ
i+1
= −1
◦
rather than 359
◦
. This angle alignment can
be done by comparing angle estimation in current frame to the one in previous frame. If
the angle di fference is greater than 90
◦
, which is the biggest angle di fference assumption
between frames, than add or subtract 360
◦
to the angle in current time frame for multiple
times to make sure that consecutive angle estimations are within 90
◦
range. At this stage,
step 1 is complete for direction unit vector and angle estimation.
180
X
Y
Z
n
^
α
1
α
2
β
1
β
2
Figure C.2: Non-unique representations of same direction vector.
For step 2, angle ϕ
1
and ϕ
2
are definedinFig.C.1. ϕ
1
is evaluated from positive
z axis with range from 0 to 2 π,and ϕ
2
is evaluated from x − z plan with range from 0
to 2 π. With this range setting, for each direction unit vector there are two possible angle
sets. Figure C.2 shows that two sets of angles, ( α
1
,α
2
) and ( β
1
,β
2
), can express the same
direction. This again causes inconsistent estimations between time frames.
In Fig C.3, ˆ n
t
and ˆ n
t+1
are unit vectors of two successive time frames. ϕ
( t)
1
and
ϕ
( t)
2
are pre-determined at time t,and ϕ
( t+1)
1
and ϕ
( t+1)
2
need to be evaluated according to
ϕ
( t)
1
and ϕ
( t)
2
. The two angle sets correspond to ϕ
( t+1)
1
and ϕ
( t+1)
2
are ( α
1
,α
2
) and ( β
1
,β
2
)
as seen in Fig. C.3. If angle set ( α
1
,α
2
) is chosen, that means between time t and t+1,
ϕ
2
doesn’t change much, and yet ϕ
1
changes almost 180
◦
.Ifangleset ( β
1
,β
2
) is chosen,
both changes in ϕ
1
and ϕ
2
are in reasonable range (within 90
◦
). C
α
and C
β
in Fig. C.4 are
the contours of direction change between time frame t and t+1 associated with angle sets
( α
1
,α
2
) and ( β
1
,β
2
) respectively. In this case, it is more reasonable to choose C
β
as the
direction change contour.
181
X
Y
Z
α
1
α
2
β
1
β
2
φ
1
(t)
φ
2
() t
n
^
t
n
^
t+1
Figure C.3: Direction unit vectors of two successive time frame
The evaluation procedures of ϕ
1
and ϕ
2
are as follows:
Step 2 procedures:
1. Fordirectionvector ˆ n
t
1
at time t
1
,assignangleset ( ϕ
( t
1
)
1
,ϕ
( t
1
)
2
).Iftheangleset
is not in [0 ,2 π] range, change angles to be in the range by add or subtract 360
◦
for multiple
times if necessary. The angle set within range is denoted as ( ϕ
( t
1
) ∗
1
,ϕ
( t
1
) ∗
2
).
2. For ˆ n
t
2
defined as
ˆ n
t
2
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
n
t
2
,x
n
t
2
,y
n
t
2
,z
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.5)
, determine the projection on x − z plan and evaluate the angles α
1
and β
1
from positive
z axis as shown in Fig. C.5, where α
1
is the angle between the real projection and z axis,
and β
1
= α
1
± 180
◦
.Thechoiceof ± sign depends on range of β
1
.Since ϕ
( t
1
) ∗
1
is also in
182
X
Z
n
^
t
n
^
t+1
C
α
C
β
C angle sweep
α
C angle sweep
β
Figure C.4: Angle sweep di fferences
[0 ,2 π] range, it is evident to determine which of α
1
and β
1
is close to ϕ
( t
1
) ∗
1
.Forthecase
illustrated in Fig. C.5, β
1
is closer to ϕ
( t
1
) ∗
1
.Thatis ϕ
( t
2
) ∗
1
should be β
1
.
Z
X
φ
1
(t )*
1
α
1
β
1
n x-z plan projection
^
t
n x-z plan projection
^
t+1
Figure C.5: x − z plan projection of direction vector
3. After ϕ
( t
2
) ∗
1
is determined as α
1
or β
1
,withthevalueof n
t
2
,y
, ϕ
( t
2
) ∗
2
can be
determined.
183
4. Compare angle sets ( ϕ
( t
1
)
1
,ϕ
( t
1
)
2
) and ( ϕ
( t
2
) ∗
1
,ϕ
( t
2
) ∗
2
). If the di fferences of both
anglesarewithin90
◦
,then( ϕ
( t
2
)
1
,ϕ
( t
2
)
2
)=( ϕ
( t
2
) ∗
1
,ϕ
( t
2
) ∗
2
).Otherwise, ( ϕ
( t
2
)
1
,ϕ
( t
2
)
2
)=( ϕ
( t
2
) ∗
1
±
k
1
· 2π, ϕ
( t
2
) ∗
2
± k
2
· 2 π),where k
1
and k
2
are integers to match angles in previous frame
within .90
◦
.
5. Repeat procedures above for ˆ n
t
i
with respect to its previous direction unit
vector ˆ n
t
i −1
.
Some special case needs to be noted. As shown in Fig. C.6, the direction unit
vector is aligned with y axis. For this direction, ϕ
2
is (π/2)±2kπ but ϕ
1
is undetermined.
In fact for this special case, ϕ
1
can be any angle value. If for certain time frame t
i
this
special case happens, ϕ
( t
i
)
1
can be determined by interpolating between ϕ
( t
i −1
)
1
and ϕ
( t
i+1
)
1
,
or, in this paper, ϕ
( t
i
)
1
is set to be equal to ϕ
( t
i −1
)
1
, which means only ϕ
2
changes between
these two time frame and ϕ
1
stays the same.
X
Z
Y
φ u
1
ndetermined
φ = π/2
2
n
^
Figure C.6: Special case of direction vector
After step 1 and 2, the transformation to three-angle parameterization is done,
and interpolation of ϕ
1
, ϕ
2
and θ can be calculated using cubic spline function. Once the
184
time function of all three angles are provided, quaternion time function can be determined
for dynamic analysis.
C.3 Estimation of First Derivatives of Three Angles
In ordertoevaluateinitial guessofangularvelocityofa bodyfromattitudes, after
three angles [ ϕ
1
,ϕ
2
,θ] are calculated from quaternion in each time frame, the interpolation
ofthreeanglescanbepreformed. Equation. (B.41)istheformulatoestimateinitialangular
velocity ω,where
˙
ˆ ncanbeestimatedinEq. (??)providing θ 6=2 nπ,and
˙
θ canbeestimated
from θ interpolation.
³
ω
b
I
´
I
= −
˙
θˆ n −sin θ
˙
ˆ n −(1 −cos θ)ˆ n×
˙
ˆ n (C.6)
˙
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
−sin ϕ
2
sin ϕ
1
· ˙ ϕ
2
+cos ϕ
2
cos ϕ
1
· ˙ ϕ
1
cos ϕ
2
· ˙ ϕ
2
−sin ϕ
2
cos ϕ
1
· ˙ ϕ
2
−cos ϕ
2
sin ϕ
1
· ˙ ϕ
1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.7)
If θ=2 nπ, Eq. (B.41) becomes
³
ω
b
I
´
I
= −
˙
θˆ n (C.8)
which means
¡
ω
b
I
¢
I
k ˆ n,and
˙
ˆ n=[0 ,0 ,0]
T
.
After the initial guess of angular velocity being input into Algo 2, the new set
of estimation angular velocities ω
es t
i
which satisfy all constraints can be found. From here
anewsetof first derivatives of three angles need to be evaluated in order to have second
derivativesofthreeangleswhichleadtotheinitialguessesofangularaccelerationsofbodies
for Algo 3.
185
Ifthreeanglesandangularvelocity ω areknown, theestimationoffirstderivatives
ofthreeanglesisasfollow:
From Eq. (B.37), we have
˙
θ = −ˆ n·
³
ω
b
I
´
b
= −ˆ n·
³
ω
b
I
´
I
Since ˆ n and
¡
ω
b
I
¢
I
are known at this stage,
˙
θ can be evaluated, thus in Eq. (B.41)
˙
ˆ n=[ ˙ n
x
, ˙ n
y
, ˙ n
z
] can be evaluated, too. From the evaluation of
˙
ˆ n,inEq. (C.7) ˙ ϕ
1
and ˙ ϕ
2
can be calculated.
Case 1: If ˙ n
y
=0,then(i) cos ϕ
2
=0 or (ii) ˙ ϕ
2
=0 and cos ϕ
2
6=0
For (i), from Eq. (C.7),
˙
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙ n
x
˙ n
y
˙ n
z
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
−sin ϕ
1
· ˙ ϕ
2
0
−cos ϕ
1
· ˙ ϕ
2
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.9)
Since sin ϕ
1
and cos ϕ
1
can not both be zeros, ˙ ϕ
2
can be calculated as
˙ ϕ
2
= −
˙ n
x
sin ϕ
1
when sin ϕ
1
6=0
(C.10)
˙ ϕ
2
= −
˙ n
z
cos ϕ
1
when cos ϕ
1
6=0
(C.11)
When cos ϕ
2
=0, ˙ ϕ
1
can not be evaluated, thus finite di fference method is used
to evaluate ˙ ϕ
1
.
˙ ϕ
1
=
1
2
Ã
ϕ
( t+1)
1
− ϕ
( t −1)
1
∆ t
!
(C.12)
where ∆ t is time interval between frames.
186
For (ii), from Eq. (C.7),
˙
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙ n
x
˙ n
y
˙ n
z
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
cos ϕ
2
cos ϕ
1
· ˙ ϕ
1
0
−cos ϕ
2
sin ϕ
1
· ˙ ϕ
1
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.13)
then,
˙ ϕ
1
=
˙ n
x
cos ϕ
2
cos ϕ
1
when cos ϕ
1
6=0
(C.14)
˙ ϕ
1
=
− ˙ n
z
cos ϕ
2
sin ϕ
1
when cos ϕ
1
=0
(C.15)
Case 2: If ˙ n
y
6=0
˙ ϕ
2
=
˙ n
y
cos ϕ
2
(C.16)
˙ ϕ
1
=
˙ n
x
+sin ϕ
2
sin ϕ
1
· ˙ ϕ
2
cos ϕ
2
cos ϕ
1
when cos ϕ
1
6=0
(C.17)
˙ ϕ
1
=
˙ n
z
+sin ϕ
2
cos ϕ
1
· ˙ ϕ
2
−cos ϕ
2
sin ϕ
1
when cos ϕ
1
=0
(C.18)
C.4 Estimation of Second Derivatives of Three Angles
From Eqs. (B.37) and (B.41), by di fferentiation, it leads to
¨
θ = −
˙
ˆ n·
³
ω
b
I
´
I
− ˆ n·
³
α
b
I
´
I
(C.19)
³
α
b
I
´
I
= −
¨
θ· ˆ n −(1+cos θ)
˙
θ·
˙
ˆ n −sin θ·
¨
ˆ n
−(sin θ)
˙
θ· ˆ n×
˙
ˆ n −(1 −cos θ)· ˆ n×
¨
ˆ n (C.20)
where
¡
α
b
I
¢
I
is the angular acceleration of a body. When initial guess of
¨
θ is evaluated from
interpolation of
˙
θ after Algo 2, α can be calculated in Eq. (C.19), and later in Eq. (C.20)
187
¨
ˆ n can be evaluated providing that θ 6=2 nπ.If θ=2 nπ,theninEq. (C.20)
˙
ˆ n=0,and
α = −
¨
θˆ n.Thatis α k ˆ n and thus
¨
ˆ n=0.
For other values of θ,therelationshipsof
¨
ˆ n and [¨ ϕ
1
, ¨ ϕ
2
] are as follows
¨
ˆ n =
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
¨ n
x
¨ n
y
¨ n
z
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(C.21)
¨ n
x
=cos ϕ
2
cos ϕ
1
¨ ϕ
1
−sin ϕ
2
sin ϕ
1
¨ ϕ
2
(C.22)
−2sin ϕ
2
cos ϕ
1
˙ ϕ
1
˙ ϕ
2
−cos ϕ
2
sin ϕ
1
¡
˙ ϕ
2
1
+ ˙ ϕ
2
2
¢
¨ n
y
= −sin ϕ
2
˙ ϕ
2
2
+cos ϕ
2
¨ ϕ
2
(C.23)
¨ n
z
= −cos ϕ
2
sin ϕ
1
¨ ϕ
1
−sin ϕ
2
cos ϕ
1
¨ ϕ
2
+2sin ϕ
2
sin ϕ
1
˙ ϕ
1
˙ ϕ
2
−cos ϕ
2
cos ϕ
1
¡
˙ ϕ
2
1
+ ˙ ϕ
2
2
¢
(C.24)
[¨ ϕ
1
,¨ ϕ
2
] are evaluated in two di fferent cases.
Case 1: cos ϕ
2
=0
In this case, from Eq. (C.23) the value of ¨ ϕ
2
can not be calculated. From Eqs.
(C.22) and (C.24), it follows
¨ ϕ
2
=
¨ n
x
+2sin ϕ
2
cos ϕ
1
˙ ϕ
1
˙ ϕ
2
−sin ϕ
2
sin ϕ
1
when sin ϕ
1
6=0
(C.25)
¨ ϕ
2
=
¨ n
z
−2sin ϕ
2
sin ϕ
1
˙ ϕ
1
˙ ϕ
2
−sin ϕ
2
cos ϕ
1
when sin ϕ
1
=0
(C.26)
For ¨ ϕ
1
, finite di fference method is used again
¨ ϕ
1
=
1
2
Ã
˙ ϕ
( t+1)
1
− ˙ ϕ
( t −1)
1
∆ t
!
(C.27)
Case 2: cos ϕ
2
6=0
188
In Eqs. (C.22-C.24)
¨ ϕ
2
=
¨ n
y
+sin ϕ
2
˙ ϕ
2
2
cos ϕ
2
(C.28)
when cos ϕ
1
6=0
¨ ϕ
1
=[¨ n
x
+sin ϕ
2
sin ϕ
1
¨ ϕ
2
+2sin ϕ
2
cos ϕ
1
˙ ϕ
1
˙ ϕ
2
+cos ϕ
2
sin ϕ
1
¡
˙ ϕ
2
1
+ ˙ ϕ
2
2
¢
](cos ϕ
2
cos ϕ
1
)
−1
(C.29)
when cos ϕ
1
=0
¨ ϕ
1
=[¨ n
z
+sin ϕ
2
cos ϕ
1
¨ ϕ
2
−2sin ϕ
2
sin ϕ
1
˙ ϕ
1
˙ ϕ
2
+cos ϕ
2
cos ϕ
1
¡
˙ ϕ
2
1
+ ˙ ϕ
2
2
¢
](cos ϕ
2
sin ϕ
1
)
−1
(C.30)
189
Appendix D
Quaternion Identity and Rotation
Matrix
In this section some indentities of quaternion are derived. The results are from
[48] with modifications. These identities are useful for dynamic formulation of quater-
nion as rotation variable. Although 3D quaternion rotation formulation is standardized by
Nikravesh[], however, in this work the notations are di fferent. First, defined quaternion q
as
q =
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
1
q
2
q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(D.1)
where the vector part q of quaternion is on top of the scalar part q
4
, which is di fferent than
thedefinitioninNikravesh’sbook. Quaternion q expressedinEq. (D.1)istheabbreviation
of q
b
I
, which means the passive rotation transferring a fixed vector from frame { b} to frame
190
{ I}. From Eq. (B.17), quaternion product in matrix form can be written as
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
4
q
3
− q
2
q
1
− q
3
q
4
q
1
q
2
q
2
− q
1
q
4
q
3
− q
1
− q
2
− q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
−[[q]]+ q
4
I
3
q
−q
T
q
4
⎤
⎥
⎥
⎦
= { q}
L
(D.2)
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
4
− q
3
q
2
q
1
q
3
q
4
− q
1
q
2
− q
2
q
1
q
4
q
3
− q
1
− q
2
− q
3
q
4
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
[[q]]+ q
4
I
3
q
−q
T
q
4
⎤
⎥
⎥
⎦
= { q}
R
(D.3)
From Eqs. (B.28-B.30) and the definition of quaternion product, the rotation of a vector
v can be expressed as
q ⊗ v ⊗ q
∗
= { q}
L
{ q
∗
}
R
· v (D.4)
=
⎡
⎢
⎢
⎣
R
0
0
T
1
⎤
⎥
⎥
⎦
· v (D.5)
where v =
£
v
T
,0
¤
T
,and
0=[0 ,0 ,0]
T
,and Risarotationmatrixwhichrepresentsthesame
rotation as q. From Eqs. (D.2-D.5), the following 3×4 matrices can be defined:
G =[ −[[q]]+ q
4
I
3
, −q]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
4
q
3
− q
2
− q
1
− q
3
q
4
q
1
− q
2
q
2
− q
1
q
4
− q
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(D.6)
L =[[[q]]+ q
4
I
3
, −q]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
q
4
− q
3
q
2
− q
1
q
3
q
4
− q
1
− q
2
− q
2
q
1
q
4
− q
3
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(D.7)
191
Eq. (D.15) is the exact relationship between quaternion and rotation matrix.
Each row of G and L is orthogonal to q, i.e.,
Gq =[ −[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
0 (D.8)
Lq =[[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
=
0 (D.9)
A direct calculation reveals that the rows of G are orthogonal, as well as L
GG
T
= I
3
(D.10)
LL
T
= I
3
(D.11)
GG
T
= LL
T
(D.12)
and an interesting relationship
GL
T
=[ −[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
−[[q]]+ q
4
I
3
−q
T
⎤
⎥
⎥
⎦
(D.13)
=( q
2
4
−q·q) I
3
+2qq
T
−2 q
4
[[q]] (D.14)
which corresponds to
R=( q
2
4
−q·q) I
3
+2qq
T
−2 q
4
[[q]] (D.15)
,thus
R = GL
T
(D.16)
192
In this work, R = R
b
I
which can transform a vector from frame { b} to frame { I} .Another
property of G and L is
G
T
G = −qq
T
+ I
4
(D.17)
L
T
L = −qq
T
+ I
4
(D.18)
G
T
G = L
T
L (D.19)
which are duals of Eqs. (D.10-D.12). Eq.(D.16) demonstrates that the quadratic transfor-
mation matrix R can be treated as the result of two successive linear transformations. This
is one of the most useful relationships between Gand L matrices and is a powerful property
of quaternion.
The unity constraint of quaternion can leads to the constraint of derivative of
quaternion.
q
T
q=1 (D.20)
˙ q
T
q + q
T
˙ q=0 (D.21)
˙ q
T
q=0 (D.22)
From Eqs. (D.8) and (D.9),
d
dt
( Gq)=
d
dt
(Lq)=0 (D.23)
˙
Gq + G ˙ q =0 (D.24)
˙
Lq + L ˙ q =0 (D.25)
˙
Gq = − G ˙ q (D.26)
˙
Lq = − L ˙ q (D.27)
193
The product of
˙
G ˙ q can be calculated, using Eq. (D.6)
˙
G ˙ q =[ −[[˙ q]]+ ˙ q
4
I
3 ,
− ˙ q]
⎡
⎢
⎢
⎣
˙ q
q
4
⎤
⎥
⎥
⎦
= −[[˙ q]] ˙ q+ ˙ q
4
˙ q − q
4
˙ q
=
0 (D.28)
and similar result can be obtain for
˙
L ˙ q
˙
L ˙ q =
0 (D.29)
From Eqs. (D.6) and (D.7),
G
˙
L
T
=
˙
GL
T
(D.30)
, so the derivative of rotation matrix R can be estimated as
˙
R =
d
dt
¡
GL
T
¢
=
˙
GL
T
+ G
˙
L
T
=2
˙
GL
T
=2 G
˙
L
T
(D.31)
The product of G ˙ q can be expand as follows
G ˙ q =[ −[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
˙ q
˙ q
4
⎤
⎥
⎥
⎦
= −[[q]] ˙ q+ q
4
˙ q − ˙ q
4
q (D.32)
194
and it can be verified by direct calculation that
[[ a]][[ b]] = ba
T
− a
T
bI
3
(D.33)
[[([[ a]]· b)]] = ba
T
− ab
T
(D.34)
=[[ a]][[ b]] −[[ b]][[ a]] (D.35)
[[ a]][[ b]]+ ab
T
=[[ b]][[ a]]+ ba
T
(D.36)
then, for [[ G ˙ q]]
[[ G ˙ q]] = [[ −[[q]] ˙ q]]+ q
4
[[˙ q]] − ˙ q
4
[[q]]
= −([[q]][[˙ q]] −[[˙ q]][[q]])+ q
4
[[˙ q]] − ˙ q
4
[[q]]
=[ −[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
[[˙ q]]+ ˙ q
4
I
3
−˙ q
T
⎤
⎥
⎥
⎦
= G
˙
G
T
(D.37)
and
[[ L ˙ q]] = − L
˙
L
T
(D.38)
From Eqs. (D.10) and (D.11)
d
dt
GG
T
=
d
dt
LL
T
=0
G
˙
G
T
= −
˙
GG
T
(D.39)
L
˙
L
T
= −
˙
LL
T
(D.40)
and from Eq. (D.22)
d
dt
¡
˙ q
T
q
¢
=0 (D.41)
˙ q
T
˙ q + q
T
¨ q =0 (D.42)
195
From Eq. (D.31)
d
dt
˙
R =
¨
R
=2
¨
GL
T
+2
˙
G
˙
L
T
(D.43)
=2 G
¨
L
T
+2
˙
G
˙
L
T
(D.44)
and
¨
GL
T
= G
¨
L
T
(D.45)
D.1 Identities with Arbitrary Vectors
Let a be an arbitrary vector,
G
T
a =
⎡
⎢
⎢
⎣
[[q]]+ q
4
I
3
−q
T
⎤
⎥
⎥
⎦
· a
=
⎡
⎢
⎢
⎣
[[q]] a+ q
4
a
−q
T
a
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
−[[ a]]q+ q
4
a
− a
T
q
⎤
⎥
⎥
⎦
=
⎡
⎢
⎢
⎣
−[[ a]] a
− a
T
0
⎤
⎥
⎥
⎦
⎡
⎢
⎢
⎣
q
q
4
⎤
⎥
⎥
⎦
(D.46)
Define
−
a =
⎡
⎢
⎢
⎣
−[[ a]] a
− a
T
0
⎤
⎥
⎥
⎦
and
+
a =
⎡
⎢
⎢
⎣
[[ a]] a
− a
T
0
⎤
⎥
⎥
⎦
(D.47)
,then
G
T
a =
−
aq and L
T
a =
+
aq
(D.48)
196
and also
+
a
T
= −
+
aand
−
a
T
= −
−
a
(D.49)
Some relationships are also useful when constructing dynamic equations.
G
−
a = −[[ a]] G+ aq
T
(D.50)
L
+
a =[[ a]] L+ aq
T
(D.51)
From Eq. (D.48)
˙
G
T
a+ G
T
˙ a =
−
˙ aq +
−
a ˙ q (D.52)
and utilize Eq. (D.48) again since a is arbitrary, substitute a as ˙ a
G
T
˙ a =
−
˙ aq (D.53)
Then from Eq. (D.52)
˙
G
T
a =
−
a ˙ q (D.54)
and similarly
˙
L
T
a =
+
a ˙qand L
T
˙ a =
+
˙ aq
(D.55)
For derivative of rotation matrix R and an arbitrary vector a
˙
Ra =2
˙
GL
T
a=2 G
˙
L
T
a (D.56)
=2
˙
G
+
aq=2 G
+
a ˙ q (D.57)
and
˙
R
T
a =2
˙
LG
T
a=2 L
˙
G
T
a (D.58)
=2
˙
L
−
aq=2 L
−
a ˙ q (D.59)
197
From Eqs. (D.54) and (D.55)
¨
G
T
a+
˙
G
T
˙ a =
−
˙ a ˙ q +
−
a¨ q (D.60)
¨
L
T
a+
˙
L
T
˙ a =
+
˙ a ˙ q +
+
a¨ q (D.61)
and again since a is arbitrary, use ˙ a
˙
G
T
˙ a =
−
˙ a ˙ q (D.62)
˙
L
T
˙ a =
+
˙ a ˙ q (D.63)
, then Eqs. (D.60) and (D.61) become
¨
G
T
a =
−
a¨ q (D.64)
¨
L
T
a =
+
a¨ q (D.65)
Back to rotation matrix and consider second order derivative from Eqs. (D.43) and (D.44)
¨
Ra =2
¨
GL
T
a+2
˙
G
˙
L
T
a=2
¨
G
T
+
aq+2
˙
G
+
a ˙ q (D.66)
=2 G
¨
L
T
a+2
˙
G
˙
L
T
a=2
˙
G
+
a ˙ q+2 G
+
a¨ q (D.67)
and
¨
R
T
a=2 L
¨
G
T
a+2
˙
L
˙
G
T
a=2 L
−
a¨ q+2
˙
L
−
a ˙ q (D.68)
198
The partial derivative of the matrix product Ra with respect to q is expanded as
follows:
∂
∂q
(Ra)=
∂
∂q
¡
GL
T
a
¢
=
∂
∂q
³³
q
2
4
−kqk
2
´
a+2qq
T
a −2 q
4
[[q]] a
´
=
∂
∂q
¡¡
2 q
2
4
−1
¢
a+2qq
T
a −2 q
4
[[q]] a
¢
=
£
2qa
T
+2q
T
aI
3
+2 q
4
[[ a]] ,4 q
4
a −2[[ q]] a
¤
=2[ −[[q]]+ q
4
I
3
, −q]
⎡
⎢
⎢
⎣
[[ a]] a
− a
T
0
⎤
⎥
⎥
⎦
+2 a
£
q
T
,q
4
¤
=2 G
+
a+2 aq
T
(D.69)
and similar expansion can be done with R
T
a
∂
∂q
¡
R
T
a
¢
=2 L
−
a2 aq
T
(D.70)
D.2 Identities with Angular Velocity
For angular velocity defined from the first derivative of rotation matrix in Eq.
(A.45) and Eq. (D.31)
˙
R
b
I
=
hh³
ω
b
I
´
I
ii
R
b
I
(D.71)
=2
˙
GL
T
=2 G
˙
L
T
(D.72)
hh³
ω
b
I
´
I
ii
R
b
I
³
R
b
I
´
T
=
˙
R
b
I
³
R
b
I
´
T
(D.73)
199
hh³
ω
b
I
´
I
ii
=2
˙
GL
T
LG
T
=2
˙
G( −qq
T
+ I
4
) G
T
=2
˙
GG
T
= −2 G
˙
G
T
= −2[[ G ˙ q]] (D.74)
and thus,
³
ω
b
I
´
I
= −2 G ˙ q
b
I
(D.75)
˙ q
b
I
= −
1
2
G
T
³
ω
b
I
´
I
(D.76)
and for angular velocity expressed in body frame
³
ω
b
I
´
b
= −2 L ˙ q
b
I
(D.77)
˙ q
b
I
= −
1
2
L
T
³
ω
b
I
´
b
(D.78)
and the angular acceleration can be calculated from Eqs. (D.75) and (D.77)
³
˙ ω
b
I
´
I
= −2
˙
G ˙ q
b
I
−2 G¨ q
b
I
= −2 G¨ q
b
I
(D.79)
³
˙ ω
b
I
´
b
= −2
˙
L ˙ q
b
I
−2 L¨ q
b
I
= −2 L¨ q
b
I
(D.80)
TheinverseofEqs. (D.79)and(D.80)canalsobederivedbyutilizingEqs. (D.22),
(D.42), (D.76), and (D.78)
¨ q
b
I
= −
1
2
G
T
³
˙ ω
b
I
´
I
−
1
4
∙
³
ω
b
I
´
T
I
³
ω
b
I
´
I
¸
· q
b
I
(D.81)
¨ q
b
I
= −
1
2
L
T
³
˙ ω
b
I
´
b
−
1
4
∙
³
ω
b
I
´
T
b
³
ω
b
I
´
b
¸
· q
b
I
(D.82)
200
It is clear that
¡
ω
b
I
¢
T
I
¡
ω
b
I
¢
I
=
¡
ω
b
I
¢
T
b
¡
ω
b
I
¢
b
= Ω
2
,where Ω is the magnitude of ω.Further-
more, it can be shown that the scalar product
³
ω
b
I
´
T
I
³
ω
b
I
´
I
− Ω
2
=0 (D.83)
³
ω
b
I
´
T
b
³
ω
b
I
´
b
− Ω
2
=0 (D.84)
andtheseyieldto
4 ˙ q
T
˙ q − Ω
2
=0 (D.85)
201
Appendix E
Computation of Kinematic
Estimation
E.1 Computation of Angular Velocities
Let J
ω
be the objective function in Eq. (5.21). Then.
1
2
∂J
ω
∂ω
k
=( ω
k
− ω
∗
k
)
+ V
1
·
¡
− J
T
k
¢
"
H
o
−
Ã
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
+ J
i
ω
i
!#
+ V
2
·
hh
R
k
o
L
k,k −1
ii
T
( ˙ r
ck −1
− ˙ r
ck
−
hh
R
k −1
o
L
k −1 ,k
ii
ω
k −1
+
hh
R
k
o
L
k,k −1
ii
ω
k
)
− V
2
·
hh
R
k
o
L
k,k+1
ii
T
( ˙ r
ck
− ˙ r
ck+1
−
hh
R
k
o
L
k,k+1
ii
ω
k
+
hh
R
k+1
o
L
k+1,k
ii
ω
k+1
) (E.1)
202
where ω
∗
k
is calculated from marker velocities in Eq. (5.13) or from the interpolation of
quaternion of orientation.
Assuming k
th
body is connected only with ( k −1)
th
and ( k+1)
th
bodies, setting
∂J
ω
∂ω
k
=0 and collecting all variables in Eq. (E.1) leads to:
ω
k
+ V
1
· J
T
k
n
X
i=1
J
i
ω
i
− V
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k −1
o
L
k −1,k
ii
ω
k −1
+ V
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k
o
L
k,k −1
ii
ω
k
+ V
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k
o
L
k,k+1
ii
ω
k
− V
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k+1
o
L
k+1,k
ii
ω
k+1
= Ω
k
(E.2)
where
Ω
k
= ω
∗
k
+ V
1
· J
T
k
Ã
H
o
−
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
!
− V
2
·
hh
R
k
o
L
k,k −1
ii
T
( ˙ r
ck −1
− ˙ r
ck
)
+ V
2
·
hh
R
k
o
L
k,k+1
ii
T
( ˙ r
ck
− ˙ r
ck+1
) (E.3)
203
In matrix form, we have:
∙
V
1
J
T
k
J
1
··· T
k −1
T
k
T
k+1
··· V
1
J
T
k
J
n
¸
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
ω
1
.
.
.
ω
k −1
ω
k
ω
k+1
.
.
.
ω
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
= Ω
k
(E.4)
where
T
k −1
= V
1
· J
T
k
J
k −1
− V
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k −1
o
L
k −1 ,k
ii
(E.5)
T
k
= V
1
· J
T
k
J
k
(E.6)
+ V
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k
o
L
k,k −1
ii
+ V
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k
o
L
k,k+1
ii
T
k+1
= V
1
· J
T
k
J
k+1
− V
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k+1
o
L
k+1 ,k
ii
(E.7)
or,
Λ
ω
·[ ω
i,col v
]=[ Ω
i,col v
] (E.8)
204
where
[ ω
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
ω
1
.
.
.
ω
k −1
ω
k
ω
k+1
.
.
.
ω
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
an d [ Ω
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
Ω
1
.
.
.
Ω
k −1
Ω
k
Ω
k+1
.
.
.
Ω
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(E.9)
Let
£
ω
∗
i,col v
¤
=
∙
( ω
∗
1
)
T
··· ( ω
∗
k −1
)
T
( ω
∗
k
)
T
( ω
∗
k+1
)
T
··· ( ω
∗
n
)
T
¸
T
(E.10)
[ ˙ r
ci,col v
]=
∙
( ˙ r
c1
)
T
··· ( ˙ r
ck −1
)
T
( ˙ r
ck
)
T
( ˙ r
ck+1
)
T
··· ( ˙ r
cn
)
T
¸
T
(E.11)
[ J]=
∙
J
1
··· J
k −1
J
k
J
k+1
··· J
n
¸
(E.12)
[ H
ω
]= H
o
−
n
X
i=1
ρ
ci
× m
i
˙ ρ
ci
(E.13)
,then
Λ
ω
= I
3 n
+ V
1
·[ J]
T
[ J]+ V
2
· C
ωL
(E.14)
[ Ω
i,col v
]=
£
ω
∗
i,col v
¤
+ V
1
·
¡
[ H
ω
]
T
[ J]
¢
T
+ V
2
· C
ωR
·[ ˙ r
ci,col v
] (E.15)
Let J
i
=( J
i
)
o
, which denotes the moment of inertia of i
th
body represented in
reference frame, and similarly ( J
i
)
i
denotes the moment of inertia of i
th
body represented
205
in i
th
body frame. Then,
J
i
=( J
i
)
o
=( R
i
o
)( J
i
)
i
( R
i
o
)
T
(E.16)
The 3 n×3 n matrix C
ωL
in Eq. (E.14) is related to joint velocity constraints and
serves as a mapping between bodies. C
ωL
( i, j) denotes the submatrix on i
th
row block and
j
th
column block. For general cases of branched chain, we have:
C
ωL
(i, i)=
P
u
[[ R
i
o
L
iu
]]
T
[[ R
i
o
L
iu
]], where index u indicates which body connecting
to i
th
body.
C
ωL
(i, j)= C
ωL
(j, i)= −[[ R
i
o
L
ij
]]
T
[[ R
j
o
L
ji
]],if i
th
and j
th
bodies are connected.
C
ωL
(i, j)= C
ωL
(j, i)=0
3×3
,if i
th
and j
th
bodies are not connected.
For k
th
body in an open chain without branches, u=( k −1) and ( k+1),andthe
formulation of C
ωL
isvalidatedinEq.(E.2).
The 3 n×3 n matrix C
ωR
in Eq. (E.15) is defined as follows:
C
ωR
( i, i)=
P
u
[[ R
i
o
L
iu
]]
T
, where index u indicates which body connecting to i
th
body.
C
ωR
( i, j)= C
ωR
( j, i)= −[[ R
i
o
L
ij
]]
T
,if i
th
and j
th
bodies are connected.
C
ωR
( i, j)= C
ωR
( j, i)=0
3×3
,if i
th
and j
th
bodies are not connected.
Given the weightings V
1
and V
2
, the solutions for ω
i
are given by:
[ ω
i,col v
]= Λ
−1
ω
·[ Ω
i,col v
] (E.17)
206
E.2 Computation of Linear Velocities of CM
Let J
CM v
be the objective function to be minimized in Eq. (5.22), we have
1
2
∂J
CM v
∂ ˙ r
ck
=( ˙ r
ck
− ˙ r
∗
ck
)
− V
3
· m
k
Ã
M ˙ r
c
−
n
X
i=1
m
i
˙ r
ci
!
− V
4
· m
k
[[ ρ
ck
]]
T
"
H
o
−
n
X
i=1
J
i
ω
i
+[[ ρ
ci
]] m
i
( ˙ r
ci
− ˙ r
c
)
#
− V
5
·
³
˙ r
ck −1
− ˙ r
ck
+
˙
R
k −1
o
L
k −1,k
−
˙
R
k
o
L
k,k −1
´
+ V
5
·
³
˙ r
ck
− ˙ r
ck+1
+
˙
R
k
o
L
k,k+1
−
˙
R
k+1
o
L
k+1 ,k
´
(E.18)
where ˙ r
∗
ck
is the initial guess calculated in Eq. (5.5), or by interpolation of estimated r
ci
values through all time frames. Collecting variable terms, we have:
˙ r
ck
+ V
3
· m
k
n
X
i=1
m
i
˙ r
ci
+ V
4
· m
k
[[ ρ
ck
]]
T
n
X
i=1
[[ ρ
ci
]] m
i
˙ r
ci
− V
5
·( ˙ r
ck −1
− ˙ r
ck
)
+ V
5
·( ˙ r
ck
− ˙ r
ck+1
)
=
˙
Γ
k
(E.19)
where
˙
Γ
k
= ˙ r
∗
ck
+ V
3
· m
k
M ˙ r
c
+ V
4
· m
k
[[ ρ
ck
]]
T
Ã
H
o
−
n
X
i=1
J
i
ω
i
+
n
X
i=1
[[ ρ
ci
]] m
i
˙ r
c
!
+ V
5
·
³
˙
R
k −1
o
L
k −1,k
−
˙
R
k
o
L
k,k −1
´
− V
5
·
³
˙
R
k
o
L
k,k+1
−
˙
R
k+1
o
L
k+1,k
´
(E.20)
207
In matrix form, we have:
∙
T
∗
1
··· T
∗
k −1
T
∗
k
T
∗
k+1
··· T
∗
n
¸
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙ r
1
.
.
.
˙ r
k −1
˙ r
k
˙ r
k+1
.
.
.
˙ r
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
=
˙
Γ
k
(E.21)
where
T
∗
1
= V
3
m
k
m
1
I
3
+ V
4
m
k
m
1
[[ ρ
ck
]]
T
[[ ρ
c1
]] (E.22)
T
∗
k −1
= V
3
m
k
m
k −1
I
3
+ V
4
m
k
m
k −1
[[ ρ
ck
]]
T
[[ ρ
ck −1
]] − V
5
I
3
(E.23)
T
∗
k
= V
3
m
2
k
I
3
+ V
4
m
2
k
[[ ρ
ck
]]
T
[[ ρ
ck
]]+2 V
5
I
3
+ I
3
(E.24)
T
∗
k+1
= V
3
m
k
m
k+1
I
3
+ V
4
m
k
m
k+1
[[ ρ
ck
]]
T
[[ ρ
ck+1
]] − V
5
I
3
(E.25)
T
∗
n
= V
3
m
k
m
n
I
3
+ V
4
m
k
m
n
[[ ρ
ck
]]
T
[[ ρ
cn
]] (E.26)
For i=1,...n,anequationcanbeformedwithrespectto ˙ r
ci
,andthese nequations
can be written in the following form:
Λ
v
·[ ˙ r
ci,col v
]=[
˙
Γ
i,col v
] (E.27)
208
where
[ ˙ r
ci,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙ r
c1
.
.
.
˙ r
ck −1
˙ r
ck
˙ r
ck+1
.
.
.
˙ r
cn
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
and [
˙
Γ
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙
Γ
1
.
.
.
˙
Γ
k −1
˙
Γ
k
˙
Γ
k+1
.
.
.
˙
Γ
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(E.28)
Let
£
˙ r
∗
ci,col v
¤
=
∙
( ˙ r
∗
c1
)
T
··· ( ˙ r
∗
ck −1
)
T
( ˙ r
∗
ck
)
T
( ˙ r
∗
ck+1
)
T
··· ( ˙ r
∗
cn
)
T
¸
T
(E.29)
[mI]=
∙
m
1
I
3
··· m
k −1
I
3
m
k
I
3
m
k+1
I
3
··· m
n
I
3
¸
(E.30)
[mρ]=
∙
m
1
[[ ρ
c1
]] ··· m
k
[[ ρ
ck
]] ··· m
n
[[ ρ
cn
]]
¸
(E.31)
[ H
v
]= H
o
−
n
X
i=1
J
i
ω
i
+
n
X
i=1
[[ ρ
ci
]] m
i
˙ r
c
(E.32)
,then
Λ
v
= I
3 n
+ V
3
·[mI]
T
[mI]+ V
4
·[mρ]
T
[mρ]+ V
5
· C
vL
(E.33)
[
˙
Γ
i,col v
]=
£
˙ r
∗
ci,col v
¤
+ (E.34)
V
3
·[ mI]
T
M ˙ r
c
+ V
4
·[ mρ]
T
[ H
v
]+ V
5
· C
vR
(E.35)
where C
vL
is a 3 n×3 n matrix and C
vR
is a 3 n×1 column vector. Let C
vL
(i, j) denotes the
submatrix on i
th
row block and j
th
column block, and each submatrix is 3×3 in dimension,
we have:
C
vL
(i, i)= w· I
3
,where w is the total number of bodies connected to i
th
body.
209
C
vL
(i, j)= C
vL
(j, i)= − I
3
,if i
th
and j
th
bodies are connected.
C
vL
(i, j)= C
vL
(j, i)=0
3×3
,if i
th
and j
th
bodies are not connected.
Let C
vR
( i) denote the i
th
subcolumn vector, and each subvector is 3×1 in dimen-
sion.
C
vR
( i)=
P
u
−
˙
R
i
o
L
iu
+
˙
R
u
o
L
ui
, where index u indicates which body connecting to
i
th
body.
Given weightings V
3,
V
4
and V
5
, the solution of ˙ r
ci
is given by:
[ ˙ r
ci,col v
]= Λ
−1
v
·[
˙
Γ
i,col v
] (E.36)
210
E.3 Computation of Angular Accelerations
Let J
α
be the objective function in Eq. (5.23), and again an open rigid body chain
is assumed, then.
1
2
∂J
α
∂α
k
=( α
k
− α
∗
k
)
+ S
1
·( J
k
)
T
Ã
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
+ J
i
α
i
!
+ S
2
·
hh
R
k
o
L
k,k −1
ii
T
·
(¨ r
ck −1
− ¨ r
ck
+[[ ω
k −1
]][[ ω
k −1
]]· R
k −1
o
L
k −1 ,k
−[[ ω
k
]][[ ω
k
]]· R
k
o
L
k,k −1
−
hh
R
k −1
o
L
k −1 ,k
ii
α
k −1
+
hh
R
k
o
L
k,k −1
ii
α
k
)
− S
2
·
hh
R
k
o
L
k,k+1
ii
T
·
(¨ r
ck
− ¨ r
ck+1
+[[ ω
k
]][[ ω
k
]]· R
k
o
L
k,k+1
−[[ ω
k+1
]][[ ω
k+1
]]· R
k+1
o
L
k+1 ,k
−
hh
R
k
o
L
k,k+1
ii
α
k
+
hh
R
k+1
o
L
k+1 ,k
ii
α
k+1
) (E.37)
where α
∗
k
is the angular acceleration initial guess.
211
Collecting all variables in Eq. (E.37) leads to:
α
k
+ S
1
· J
T
k
n
X
i=1
J
i
α
i
− S
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k −1
o
L
k −1,k
ii
α
k −1
+ S
2
·
hh
R
k
o
L
k,k −1
ii
T
hh
R
k
o
L
k,k −1
ii
α
k
+ S
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k
o
L
k,k+1
ii
α
k
− S
2
·
hh
R
k
o
L
k,k+1
ii
T
hh
R
k+1
o
L
k+1,k
ii
α
k+1
=
˙
Ω
k
(E.38)
where
˙
Ω
k
= α
∗
k
− S
1
· J
T
k
Ã
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
!
− S
2
·
hh
R
k
o
L
k,k −1
ii
T
·
(¨ r
ck −1
− ¨ r
ck
+[[ ω
k −1
]][[ ω
k −1
]] R
k −1
o
L
k −1 ,k
−[[ ω
k
]][[ ω
k
]] R
k
o
L
k,k −1
)
+ S
2
·
hh
R
k
o
L
k,k+1
ii
T
·
(¨ r
ck
− ¨ r
ck+1
+[[ ω
k
]][[ ω
k
]] R
k
o
L
k,k+1
−[[ ω
k+1
]][[ ω
k+1
]] R
k+1
o
L
k+1 ,k
) (E.39)
In matrix form, for all α
i
, i=1 ,··· ,n
Λ
α
·[ α
i,col v
]=[
˙
Ω
i,col v
] (E.40)
212
where
[ α
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
α
1
.
.
.
α
k −1
α
k
α
k+1
.
.
.
α
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
and [
˙
Ω
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
˙
Ω
1
.
.
.
˙
Ω
k −1
˙
Ω
k
˙
Ω
k+1
.
.
.
˙
Ω
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(E.41)
Let
£
α
∗
i,col v
¤
=
∙
( α
∗
1
)
T
··· ( α
∗
k −1
)
T
( α
∗
k
)
T
( α
∗
k+1
)
T
··· ( α
∗
n
)
T
¸
T
(E.42)
[¨ r
ci,col v
]=
∙
(¨ r
c1
)
T
··· (¨ r
ck −1
)
T
(¨ r
ck
)
T
(¨ r
ck+1
)
T
··· (¨ r
cn
)
T
¸
T
(E.43)
[ H
α
]= −
n
X
i=1
ρ
ci
× m
i
¨ ρ
ci
+
˙
J
i
ω
i
(E.44)
,then
Λ
α
= I
3 n
+ S
1
·[ J]
T
[ J]+ S
2
· C
αL
(E.45)
[
˙
Ω
i,col v
]=
£
α
∗
i,col v
¤
+ S
1
·
¡
[ H
α
]
T
[ J]
¢
T
+ S
2
· C
αR
·[¨ r
ci,col v
]+ S
2
· sC
αR
(E.46)
In Eq. (E.45), C
αL
= C
ωL
, which is defined in Eq.(E.14), and in Eq. (E.46),
C
αR
= C
ωR
, which is definedinEq.(E.15). Let sC
αR
( i) denote the i
th
subcolumn vector,
and each subvector is 3×1 in dimension, then if follows:
213
sC
αR
( i)=
X
u
££
R
i
o
L
i,u
¤¤
T
¡
[[ ω
i
]][[ ω
i
]] R
i
o
L
i,u
−[[ ω
u
]][[ ω
u
]] R
u
o
L
u,i
¢
(E.47)
where i=1 ,··· ,n,and u is the body connected to i
th
body.
Given weightings S
1
and S
2
,wehave:
[ α
i,col v
]= Λ
−1
α
·[
˙
Ω
i,col v
] (E.48)
E.4 Computation of Linear Acceleration of CM
Let J
CM a
be the objective function to be minimized in Eq. (5.24), then.
1
2
∂J
CM a
∂¨ r
ck
=(¨ r
ck
− ¨ r
∗
ck
)
− S
3
· m
k
Ã
M¨ r
c
−
n
X
i=1
m
i
¨ r
ci
!
+ S
4
· m
k
[[ ρ
ck
]]
T
"
n
X
i=1
[[ ρ
ci
]] m
i
(¨ r
ci
− ¨ r
c
)+
˙
J
i
ω
i
+ J
i
α
i
#
− S
5
·
³
¨ r
ck −1
− ¨ r
ck
+
¨
R
k −1
o
L
k −1,k
−
¨
R
k
o
L
k,k −1
´
+ S
5
·
³
¨ r
ck
− ¨ r
ck+1
+
¨
R
k
o
L
k,k+1
−
¨
R
k+1
o
L
k+1 ,k
´
(E.49)
where ¨ r
∗
ck
is the initial guess of CM acceleration of k
th
body. Collecting variable terms, it
follows that
¨ r
ck
+ S
3
· m
k
n
X
i=1
m
i
¨ r
ci
+ S
4
· m
k
[[ ρ
ck
]]
T
n
X
i=1
[[ ρ
ci
]] m
i
¨ r
ci
+ S
5
·( −¨ r
ck −1
+2¨ r
ck
− ¨ r
ck+1
)
=
¨
Γ
k
(E.50)
214
where
¨
Γ
k
=¨ r
∗
ck
+ S
3
· m
k
M¨ r
c
+ S
4
· m
k
[[ ρ
ck
]]
T
Ã
n
X
i=1
[[ ρ
ci
]] m
i
¨ r
c
−
˙
J
i
ω
i
− J
i
α
i
!
+ S
5
·
³
¨
R
k −1
o
L
k −1,k
−
¨
R
k
o
L
k,k −1
´
− S
5
·
³
¨
R
k
o
L
k,k+1
−
¨
R
k+1
o
L
k+1 ,k
´
(E.51)
In matrix form, we have:
Λ
a
·[¨ r
ci,col v
]=[
¨
Γ
i,col v
] (E.52)
where
[¨ r
ci,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
¨ r
c1
.
.
.
¨ r
ck −1
¨ r
ck
¨ r
ck+1
.
.
.
¨ r
cn
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
and [
¨
Γ
i,col v
]=
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
¨
Γ
1
.
.
.
¨
Γ
k −1
¨
Γ
k
¨
Γ
k+1
.
.
.
¨
Γ
n
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(E.53)
Let
£
¨ r
∗
ci,col v
¤
=
∙
(¨ r
∗
c1
)
T
··· (¨ r
∗
ck −1
)
T
(¨ r
∗
ck
)
T
(¨ r
∗
ck+1
)
T
··· (¨ r
∗
cn
)
T
¸
T
(E.54)
[ H
a
]=
n
X
i=1
[[ ρ
ci
]] m
i
¨ r
c
+
˙
J
i
ω
i
+ J
i
α
i
(E.55)
215
,then
Λ
a
= I
3 n
+ S
3
·[ mI]
T
[ mI]+ S
4
·[mρ]
T
[mρ]+ S
5
· C
aL
(E.56)
[
¨
Γ
i,col v
]=
£
¨ r
∗
ci,col v
¤
+ (E.57)
S
3
·[mI]
T
M¨ r
c
+ S
4
·[ mρ]
T
[ H
a
]+ S
5
· C
aR
(E.58)
where C
aL
= C
vL
,and C
aR
( i) denote the i
th
subcolumn vector, and each subvector is 3×1
in dimension. It follows that
C
aR
( i)=
X
u
−
¨
R
i
o
L
iu
+
¨
R
u
o
L
ui
(E.59)
, where index u indicates which body connecting to i
th
body.
Given weightings S
3,
S
4
and S
5
,wehave:
[¨ r
ci,col v
]= Λ
−1
a
·[
¨
Γ
i,col v
] (E.60)
216
Appendix F
Human Body Scheme
The details of human body segment dimensions and mass distributions are listed
in Table. F.1 For moment of inertia of each segments, Table. F.2 lists the inertia radius in
principal direction in proportion of segment length.
Segment Longitudinal Mass Longitudinal
name Length(cm) (%) CM position(%)
Head 23 .25 6 .94 50 .02
Trunk 61 .68 43 .46 43 .1
Upperarm(L/R) 26 .45 /28 .88 2 .71 57 .72
Forearm(L/R) 24 /22 .8 1 .62 46 .08
Thigh(L/R) 40 .35 /38 .8 14 .16 40 .95
Shank(L/R) 36 .62 /37 .18 4 .33 44 .59
Foot(L/R) 27 .74 1 .37 44 .15
Table F.1: Segment dimensions
217
Segment Transverse r
x
Longitudinal r
y
Sagittal r
z
name (%) (%) (%)
Head 31 .5 26 .1 30 .3
Trunk 35 .8 19 .7 38 .4
Upperarm 26 .9 15 .8 28 .5
Forearm 26 .7 12 .2 27 .8
Thigh 32 .9 14 .9 32 .9
Shank 24 .9 10 .3 25 .5
Foot 25 .7 24 .5 12 .4
Table F.2: Inertial radius
The moment of inertia I of a segment is calculated as follows:
I
seg m ent
= mass·
⎡
⎢
⎢
⎢
⎢
⎢
⎢
⎣
( l· r
x
)
2
00
0( l· r
y
)
2
0
00 ( l· r
z
)
2
⎤
⎥
⎥
⎥
⎥
⎥
⎥
⎦
(F.1)
where l is the segment longitudinal length.
218
Appendix G
Human Kinematic Estimation
Results
Thissectionshowallkinematicestimationresultsofhumanjumpingmotion. The
constraints are compared to nominal set if applicable.
219
0 0.05 0.1
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle L
0 0.05 0.1
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle R
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle L
0.55 0.6 0.65
0
0.02
0.04
0.06
time(sec)
displacement(m)
ankle R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.1: Ankle joint location constraints
220
0 0.05 0.1
0
0.005
0.01
0.015
0.02
time(sec)
displacement(m)
knee L
0 0.05 0.1
0
0.01
0.02
0.03
0.04
0.05
time(sec)
displacement(m)
knee R
0.55 0.6 0.65 0.7
0
0.005
0.01
0.015
time(sec)
displacement(m)
knee L
0.55 0.6 0.65 0.7
0
0.01
0.02
0.03
0.04
0.05
time(sec)
displacement(m)
knee R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.2: Knee joint location constraints
221
0 0.05 0.1
0
0.02
0.04
0.06
time(sec)
displacement(m)
hip L
0 0.05 0.1
0
0.02
0.04
0.06
0.08
time(sec)
displacement(m)
hip R
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
0.08
0.1
time(sec)
displacement(m)
hip L
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
0.08
time(sec)
displacement(m)
hip R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.3: Hip joint location constraints
222
0 0.05 0.1
0
0.02
0.04
0.06
0.08
0.1
time(sec)
displacement(m)
wrist L
0 0.05 0.1
0
0.05
0.1
0.15
time(sec)
displacement(m)
wrist R
0.55 0.6 0.65 0.7
0
0.01
0.02
0.03
0.04
time(sec)
displacement(m)
wrist L
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
time(sec)
displacement(m)
wrist R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.4: Wrist joint location constraints
223
0 0.05 0.1
0
0.05
0.1
time(sec)
displacement(m)
elbow L
0 0.05 0.1
0
0.02
0.04
0.06
0.08
time(sec)
displacement(m)
elbow R
0.55 0.6 0.65 0.7
0
0.01
0.02
0.03
time(sec)
displacement(m)
elbow L
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
time(sec)
displacement(m)
elbow R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.5: Elbow joint location constraints
224
0 0.05 0.1
0
0.02
0.04
0.06
0.08
0.1
time(sec)
displacement(m)
shoulder L
0 0.05 0.1
0
0.02
0.04
0.06
0.08
0.1
time(sec)
displacement(m)
shoulder R
0.55 0.6 0.65 0.7
0
0.05
0.1
time(sec)
displacement(m)
shoulder L
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
0.08
0.1
time(sec)
displacement(m)
shoulder R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.6: Shoulder joint location constraints
225
0 0.05 0.1
0
0.05
0.1
time(sec)
displacement(m)
neck M
0.55 0.6 0.65 0.7
0
0.02
0.04
0.06
time(sec)
displacement(m)
neck M
0 0.05 0.1
0
1
2
3
time(sec)
velocity diff.(m/s)
d neck M
0.55 0.6 0.65 0.7
0
0.2
0.4
time(sec)
velocity diff.(m/s)
d neck M
0 0.05 0.1
0
1
2
x 10
4
time(sec)
acc diff.(m/s
2
)
dd neck M
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd neck M
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.7: Neck constraints
226
0 0.05 0.1
0
1
2
3
time(sec)
velocity diff.(m/s)
d ankle L
0 0.05 0.1
0
1
2
3
time(sec)
velocity diff.(m/s)
d ankle R
0.55 0.6 0.65 0.7
0
0.5
1
1.5
time(sec)
velocity diff.(m/s)
d ankle L
0.55 0.6 0.65 0.7
0
1
2
3
time(sec)
velocity diff.(m/s)
d ankle R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.8: Ankle joint velocity constraints
227
0 0.05 0.1
0
0.2
0.4
0.6
0.8
time(sec)
velocity diff.(m/s)
d knee L
0 0.05 0.1
0
0.5
1
1.5
time(sec)
velocity diff.(m/s)
d knee R
0.55 0.6 0.65 0.7
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d knee L
0.55 0.6 0.65 0.7
0
0.5
1
1.5
time(sec)
velocity diff.(m/s)
d knee R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.9: knee joint velocity constraints
228
0 0.05 0.1
0
0.2
0.4
0.6
0.8
time(sec)
velocity diff.(m/s)
d hip L
0 0.05 0.1
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip R
0.55 0.6 0.65 0.7
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip L
0.55 0.6 0.65 0.7
0
0.2
0.4
0.6
0.8
1
time(sec)
velocity diff.(m/s)
d hip R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.10: Hip joint velocity constraints
229
0 0.05 0.1
0
1
2
3
4
time(sec)
velocity diff.(m/s)
d wrist L
0 0.05 0.1
0
1
2
3
4
5
time(sec)
velocity diff.(m/s)
d wrist R
0.55 0.6 0.65 0.7
0
0.5
1
1.5
2
time(sec)
velocity diff.(m/s)
d wrist L
0.55 0.6 0.65 0.7
0
1
2
3
time(sec)
velocity diff.(m/s)
d wrist R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.11: Wrist joint velocity constraints
230
0 0.05 0.1
0
1
2
3
4
time(sec)
velocity diff.(m/s)
d elbow L
0 0.05 0.1
0
1
2
3
time(sec)
velocity diff.(m/s)
d elbow R
0.55 0.6 0.65 0.7
0
0.5
1
1.5
time(sec)
velocity diff.(m/s)
d elbow L
0.55 0.6 0.65 0.7
0
0.5
1
1.5
2
2.5
time(sec)
velocity diff.(m/s)
d elbow R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.12: Elbow joint velocity constraints
231
0 0.05 0.1
0
1
2
3
4
time(sec)
velocity diff.(m/s)
d shoulder L
0 0.05 0.1
0
1
2
3
4
5
time(sec)
velocity diff.(m/s)
d shoulder R
0.55 0.6 0.65 0.7
0
0.5
1
1.5
time(sec)
velocity diff.(m/s)
d shoulder L
0.55 0.6 0.65 0.7
0
0.5
1
1.5
2
time(sec)
velocity diff.(m/s)
d shoulder R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.13: Shoulder joint velocity constraints
232
0 0.05 0.1
0
1000
2000
3000
4000
5000
time(sec)
acc diff.(m/s
2
)
dd ankle L
0 0.05 0.1
0
1
2
3
4
5
x 10
5
time(sec)
acc diff.(m/s
2
)
dd ankle R
0.55 0.6 0.65 0.7
0
100
200
300
400
500
time(sec)
acc diff.(m/s
2
)
dd ankle L
0.55 0.6 0.65 0.7
0
200
400
600
800
1000
time(sec)
acc diff.(m/s
2
)
dd ankle R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.14: Ankle joint acceleration constraints
233
0 0.05 0.1
0
1
2
3
4
5
x 10
4
time(sec)
acc diff.(m/s
2
)
dd knee L
0 0.05 0.1
0
0.5
1
1.5
2
x 10
4
time(sec)
acc diff.(m/s
2
)
dd knee R
0.55 0.6 0.65 0.7
0
50
100
150
200
time(sec)
acc diff.(m/s
2
)
dd knee L
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd knee R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.15: Knee joint acceleration constraints
234
0 0.05 0.1
0
0.5
1
1.5
2
x 10
4
time(sec)
acc diff.(m/s
2
)
dd hip L
0 0.05 0.1
0
2000
4000
6000
time(sec)
acc diff.(m/s
2
)
dd hip R
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd hip L
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd hip R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.16: Hip joint acceleration constraints
235
0 0.05 0.1
0
5000
10000
15000
time(sec)
acc diff.(m/s
2
)
dd wrist L
0 0.05 0.1
0
1000
2000
3000
time(sec)
acc diff.(m/s
2
)
dd wrist R
0.55 0.6 0.65 0.7
0
50
100
150
200
time(sec)
acc diff.(m/s
2
)
dd wrist L
0.55 0.6 0.65 0.7
0
1000
2000
3000
time(sec)
acc diff.(m/s
2
)
dd wrist R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.17: Wrist joint acceleration constraints
236
0 0.05 0.1
0
5000
10000
15000
time(sec)
acc diff.(m/s
2
)
dd elbow L
0 0.05 0.1
0
5000
10000
15000
time(sec)
acc diff.(m/s
2
)
dd elbow R
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd elbow L
0.55 0.6 0.65 0.7
0
100
200
300
time(sec)
acc diff.(m/s
2
)
dd elbow R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.18: Elbow joint acceleration constraints
237
0 0.05 0.1
0
1000
2000
3000
time(sec)
acc diff.(m/s
2
)
dd shoulder L
0 0.05 0.1
0
1000
2000
3000
4000
5000
time(sec)
acc diff.(m/s
2
)
dd shoulder R
0.55 0.6 0.65 0.7
0
100
200
300
400
500
time(sec)
acc diff.(m/s
2
)
dd shoulder L
0.55 0.6 0.65 0.7
0
100
200
300
400
500
time(sec)
acc diff.(m/s
2
)
dd shoulder R
ic
sp
p
sp
v
sp
a
sp
pva
Figure G.19: Shoulder joint acceleration constraints
238
Appendix H
Human Dynamic Estimation
Results
Thissectionshowsalltheforcesandtorquesestimationinhumanjumpingmotion.
239
0 0.02 0.04 0.06 0.08 0.1
−4
−2
0
2
x 10
−5
time(sec)
force (N)
trunk
0.55 0.6 0.65 0.7
−1.5
−1
−0.5
0
0.5
1
x 10
−6
time(sec)
force (N)
trunk
F
x
F
y
F
z
Figure H.1: External forces acting on trunk
240
0 0.05 0.1
−3
−2
−1
0
1
2
time(sec)
torque (N−m)
ankleL
0.55 0.6 0.65 0.7
−1
−0.5
0
0.5
time(sec)
torque (N−m)
ankleL
0 0.05 0.1
−6
−4
−2
0
2
4
time(sec)
torque (N−m)
ankleR
0.55 0.6 0.65 0.7
−0.6
−0.4
−0.2
0
0.2
0.4
time(sec)
torque (N−m)
ankleR
τ
x
τ
y
τ
z
Figure H.2: Torques in ankle joints
241
0 0.05 0.1
−6
−4
−2
0
time(sec)
torque (N−m)
elbowL
0.55 0.6 0.65 0.7
−0.4
−0.2
0
0.2
0.4
0.6
time(sec)
torque (N−m)
elbowL
0 0.05 0.1
−5
0
5
10
15
20
time(sec)
torque (N−m)
elbowR
0.55 0.6 0.65 0.7
−0.5
0
0.5
1
time(sec)
torque (N−m)
elbowR
τ
x
τ
y
τ
z
Figure H.3: Torques in elbow joints
242
0 0.05 0.1
−10
−5
0
5
10
15
time(sec)
torque (N−m)
hipL
0.55 0.6 0.65 0.7
−0.5
0
0.5
1
time(sec)
torque (N−m)
hipL
0 0.05 0.1
−10
−5
0
5
10
time(sec)
torque (N−m)
hipR
0.55 0.6 0.65 0.7
−1
−0.5
0
0.5
time(sec)
torque (N−m)
hipR
τ
x
τ
y
τ
z
Figure H.4: Torques in hip joints
243
0 0.05 0.1
−2
−1
0
1
2
3
time(sec)
torque (N−m)
kneeL
0.55 0.6 0.65 0.7
−0.5
0
0.5
1
time(sec)
torque (N−m)
kneeL
0 0.05 0.1
−2
0
2
4
time(sec)
torque (N−m)
kneeR
0.55 0.6 0.65 0.7
−0.5
0
0.5
time(sec)
torque (N−m)
kneeR
τ
x
τ
y
τ
z
Figure H.5: Torques in knee joints
244
0 0.05 0.1
−6
−4
−2
0
2
4
6
time(sec)
torque (N−m)
neck
0.55 0.6 0.65 0.7
−0.6
−0.4
−0.2
0
0.2
0.4
time(sec)
torque (N−m)
neck
0 0.05 0.1
−15
−10
−5
0
5
10
time(sec)
torque (N−m)
trunk
0.55 0.6 0.65 0.7
−2
−1
0
1
time(sec)
torque (N−m)
trunk
τ
x
τ
y
τ
z
Figure H.6: Torques in neck and on trunk.
245
0 0.05 0.1
−10
−5
0
5
10
15
time(sec)
torque (N−m)
shoulderL
0.55 0.6 0.65 0.7
−0.5
0
0.5
1
1.5
2
time(sec)
torque (N−m)
shoulderL
0 0.05 0.1
−20
−10
0
10
20
time(sec)
torque (N−m)
shoulderR
0.55 0.6 0.65 0.7
−1
0
1
2
time(sec)
torque (N−m)
shoulderR
τ
x
τ
y
τ
z
Figure H.7: Torques in shoulder joints
246
0 0.05 0.1
−2
−1
0
1
2
time(sec)
torque (N−m)
wristL
0.55 0.6 0.65 0.7
−0.1
−0.05
0
0.05
0.1
0.15
time(sec)
torque (N−m)
wristL
0 0.05 0.1
−3
−2
−1
0
1
2
3
time(sec)
torque (N−m)
wristR
0.55 0.6 0.65 0.7
−0.1
−0.05
0
0.05
0.1
time(sec)
torque (N−m)
wristR
τ
x
τ
y
τ
z
Figure H.8: Torques in wrist joints

Abstract (if available)

Linked assets

University of Southern California Dissertations and Theses

Conceptually similar

PDF

Dynamic modeling and simulations of rigid-flexible coupled systems using quaternion dynamics

PDF

Using nonlinear feedback control to model human landing mechanics

PDF

Modeling and vibration analysis of wheelchair-users

PDF

Modeling human regulation of momentum while interacting with the environment

PDF

On the synthesis of dynamics and control for complex multibody systems

PDF

Dynamic analysis and control of one-dimensional distributed parameter systems

PDF

Static and dynamic analysis of two-dimensional elastic continua by combined finite difference with distributed transfer function method

PDF

Control of spacecraft with flexible structures using pulse-modulated thrusters

PDF

An approach to dynamic modeling of percussive mechanisms

PDF

Magnetic induction-based wireless body area network and its application toward human motion tracking

PDF

On the synthesis of controls for general nonlinear constrained mechanical systems

PDF

Mathematical model and numerical analysis of a shape memory polymer composite cantilever beam for space applications

PDF

Design, modeling and analysis of piezoelectric forceps actuator

PDF

Terrain-following motion of an autonomous agent as means of motion planning in the unknown environment

PDF

An analytical and experimental study of evolving 3D deformation fields using vision-based approaches

PDF

A subgrid-scale model for large-eddy simulation based on the physics of interscale energy transfer in turbulence

PDF

Multiple degree of freedom inverted pendulum dynamics: modeling, computation, and experimentation

PDF

New approaches in modeling and control of dynamical systems

PDF

Control and dynamics of turning tasks with different rotation and translation requirements

PDF

The development of a mathematical model of a hybrid airship

Asset Metadata

Creator Lee, Jenchieh (author)

Core Title An approach to experimentally based modeling and simulation of human motion

School Viterbi School of Engineering

Degree Doctor of Philosophy

Degree Program Aerospace

Publication Date 12/06/2009

Defense Date 09/07/2009

Publisher University of Southern California (original), University of Southern California. Libraries (digital)

Tag 3D,Dynamic,human body modeling,kinematic,OAI-PMH Harvest,orientation estimation,quaternion,rigid body

Language English

Contributor Electronically uploaded by the author (provenance)

Advisor Flashner, Henryk (committee chair), Dravinski, Marijan (committee member), Mc Nitt-Gray, Jill (committee member), Shiflett, Geoffrey R. (committee member), Yang, Bingen (committee member)

Creator Email anoncool@gmail.com,jenchiel@usc.edu

Permanent Link (DOI) https://doi.org/10.25549/usctheses-m2787

Unique identifier UC1146186

Identifier etd-Lee-3276 (filename),usctheses-m40 (legacy collection record id),usctheses-c127-303101 (legacy record id),usctheses-m2787 (legacy record id)

Legacy Identifier etd-Lee-3276-0.pdf

Dmrecord 303101

Document Type Dissertation

Rights Lee, Jenchieh

Type texts

Source University of Southern California (contributing entity), University of Southern California Dissertations and Theses (collection)

Repository Name Libraries, University of Southern California

Repository Location Los Angeles, California

Repository Email cisadmin@lib.usc.edu