Embed Size (px)
Citation preview
ME547 Final Project
Modelling of Human Gait
December 6, 2016
Iman Vezvaei Jinyi (Raven) Xie
Amanda Kingman
December 6, 2016
1 of 58
Table of Contents
Section Page Number
Introduction and Objectives 2
Methods 5
Forward Dynamics 10
Kinematics 11
Equations of Motion 15
Constraints 16
Inverse Dynamics 19
Prosthetics Implementation 31
Results 35
Conclusion 44
References 45
Appendices 46
Appendix 1: Kinematics Equations 46
Appendix 2: Equations of Motion 50
Appendix 3: Matlab Code – Kinematics and EoMs 51
Appendix 4: Matlab Code – Constraints 56
Appendix 5: Matlab Code – Inverse Dynamics 58
December 6, 2016
2 of 58
INTRODUCTION AND OBJECTIVES
Background
There are an estimated 1.9 million amputees in the United States and approximately 185,000 amputations
surgeries performed each year. With an aging population, this number is likely to rise due to the fact that
the majority of amputations result from peripheral Vascular Disease and Diabetes. Many of these
individuals opt to use a prosthetic device to assist with the functions of daily living. However, current state
of the art in the limb replacement industry cannot match the functionality of a human limb.
Transfemoral amputees (those who lose their leg above the knee, across the femur) who use a prosthetic
leg must use approximately 80% more energy to walk than a person with two whole legs. Due to this, it
can be very difficult for transfemoral amputees to regain normal movement. Although many newer
transfemoral prosthetic legs have improved functionality by use of motors and computer
microprocessors, these devices are costly, and many continue to use simpler, more cumbersome designs.
Human Gait
“Gait” is defined by Merriam-Webster as, “a manner of walking on foot”. Human gait has been studied
abundantly for a multitude of applications for a better understanding of human movement. It is for this
reason that a simulation of human gait was chosen as an appropriate means by which to assess the
kinematics and forces associated with the human leg. See Figure 1 below for a diagram of the human gait
cycle.
Figure 1: Complete human gait cycle shown in the anatomic sagittal plane
As shown in the above diagram, there are two phases to the gait cycle for each leg: the Stance Phase, the
portion during which the foot is in contact with the walking surface, and the Swing Phase, the portion of
the gait cycle during which the foot has no contact with the ground. Throughout this report, start time (to
= 0 seconds) is defined at the moment of ‘Toe Off’ of the right leg.
December 6, 2016
3 of 58
Anatomic Definitions
Throughout this report, references are made using standard anatomic definitions.
There are three anatomic reference planes, used to describe body motion and positioning. These include
the Sagittal Plane, in the anterior-posterior direction, the Transverse Plane, in the distal-proximal
direction, and the Frontal Plane, in the medial-lateral direction. See Figure 2 below for a visual
representation of these planes.
Figure 2: Anatomic reference frames
Purpose
The purpose of this study is to conduct both forward and inverse dynamics analysis of a human leg and
prosthetic leg for comparison of kinematics, internal forces, and external forces using gait cycle data. The
results of this study may provide a means by which a comparison of the functionality of a transfemoral
prosthetic leg and a human leg may be achieved. It is our hope that an analysis of the dynamics of both
systems might create a clearer understanding of gait and functionality discrepancies between an artificial
and human leg in order to improve prosthetic leg design.
December 6, 2016
4 of 58
Objectives
The objectives of this study are outlined below:
● Create a means by which to solve for the forward dynamics of a human leg and prosthetic leg.
This includes the kinematics and equations of motion with constraints for each.
● Create a model for analysis for both a human leg and prosthetic leg. This will provide information
about inverse dynamics, including internal forces, as well as stress analysis through finite element
analysis (FEA).
● Validate solutions with existing literature.
● Assess the similarities and differences in the solutions found in the dynamics of the human leg
and prosthetic leg.
December 6, 2016
5 of 58
METHODS
Defining the Model
For the purposes of this project, in an effort to analyze the most ‘average’ circumstances for human lower
limb dynamics, the leg model is based off 50th percentile adult male anthropometric data in regards to
height, weight, leg segment lengths, and leg segment masses. The 50th percentile adult male weighs 75
kg.
The following Figure 3 shows 50th percentile anthropometric data for height and limb length segments.
These were used in the segment lengths of the model.
Figure 3: Anthropometric data for the 50th percentile male for height and limb segment lengths.
The following figure illustrates the model to be used in the multibody dynamics.
December 6, 2016
6 of 58
Figure 4: Diagram of human leg for forward dynamics analysis, including reference and local coordinate
systems.
As shown in Figure 4, the human leg is modelled as a 3-body system (thigh, shank, and foot: Body 1, Body
2, and Body 3, respectively) with local coordinate systems at each joint (hip, knee, and ankle). Each local
coordinate system aligns the 𝑛1𝑘 axis with the line of action of the leg segment, the 𝑛2
𝑘 is perpendicular to
axis 𝑛1𝑘, rotated 90° in the positive (counterclockwise) direction from 𝑛1
𝑘. Axis 𝑛3𝑘 is aligned in the medial-
lateral orientation of the system (out of the page). Each system also has a reference coordinate system,
O, shown in yellow in the above figure.
The gait movement is restricted to the sagittal plane and, assuming symmetry, it is only required to
conduct a simulation and model with just one leg. The right leg will be used in this study.
December 6, 2016
7 of 58
Data
Winter, et al. have published a complete data set for the gait cycle including information for each leg
segment and joint in regards to their position, angular velocities, and angular accelerations through the
gait cycle. This set is used as the definitive data for the project.
As an alternative, the software program Quintic Biomechanic is a biomechanics computational software
that derives data from a video using motion capture. The program is then able to export kinematic data
as well as force data. Without the full version, we were unable to simulate walking and gather data from
the program. See the images below for a sample output from Quintic Biomechanic trial software as a
proof of concept for potential implementation in the future for data harvesting. This could also be used
to measure the gait cycle of an individual with a lower limb prosthetic. This would make computations for
forward and inverse dynamics possible for the prosthetic scenario that we were unable to accomplish
without such data.
Figure M.4.1: vector representation of limb segments of an individual recorded running. The blue and
yellow identify the foot. The green identifies the shank. The red identifies the thigh.
The following are sample data outputs from the Quintic Biomechanic softwawre for a walking individual.
The first is kinematic data for the hip joint, then the knee, and last the ankle.
December 6, 2016
8 of 58
Figure M.4.2: Distance, Velocity, and Acceleration data for the Hip recorded with motion capture during
walking.
December 6, 2016
9 of 58
Figure M.4.3: Distance, Velocity, and Acceleration data for the Knee recorded with motion capture
during walking.
Figure M.4.4: Distance, Velocity, and Acceleration data for the Ankle recorded with motion capture
during walking.
December 6, 2016
10 of 58
FORWARD DYNAMICS
System
For the simplification of the system, each body of the anatomical right leg (shown previously in Figure 4)
is represented by a link segment with masses of each concentrated at the center of mass and moments
of inertia for each segment acting about the center of mass. Local limb coordinates are as shown in the
previous Figure 5.
Figure 5: Simplified human led diagram for modeling. Blue rotation arrows (positive direction) are
indicated about the hip, knee, and ankle joints. Masses for each are calculated based on a percentage of total body mass for the 50th percentile male as
are the moments of inertia. Joints are represented as hinge joints, rotating about only the n3 coordinate
axis.
December 6, 2016
11 of 58
KINEMATICS
Equations
There are three bodies in the system: the thigh, the shank, and the foot. Because each joint only allows
for rotation about a single axis, the number of degrees of freedom of this system is 3.
The generalized coordinates of the system are presented below in equations (1) and (2). The associated
position vectors and skew matrices for each body are shown in equations (3) through (7).
{𝑥} = [0 0 𝑥3 0 0 𝑥6 0 0 𝑥9] = [0 0 𝜃1 0 0 𝜃2 0 0 𝜃3] (1)
{��} = [0 0 ��3 0 0 ��6 0 0 ��9] = [0 0 ��1 0 0 ��2 0 0 ��3] (2)
��1 = 𝜉 ≅ 0 (3)
��𝑘 = [𝑙𝑘 0 0]𝑇 (4)
𝑆𝑞𝑘 = 𝑙𝑘 [0 0 0
0 0 −10 1 0
] (5)
𝑟𝑘 = [𝑙𝑘
20 0]
𝑇
(6)
𝑆𝑟1 =𝑙1
2[
0 0 00 0 −10 1 0
] (7)
The shifter matrices between each of the bodies are all the same, shown by equation (8). This is due to
the fact that they rotate about the same parallel axes at each joint.
The shifter matrices for bodies 2 and 3 with respect to the global coordinate system are calculated as
shown in equations (9) and (10).
𝑆𝑘,𝑘−1 = [ cos(𝜃𝑘) sin (𝜃𝑘) 0
−sin (𝜃𝑘) cos(𝜃𝑘) 0 0 0 1
] (8)
December 6, 2016
12 of 58
The time derivatives of the shifter matrices are displayed in equations (11), (12), and (13) accordingly.
��10 = [−sin (𝜃1) cos(𝜃1) 0
−cos (𝜃1) −sin(𝜃1) 0 0 0 0
] ∗ ��1 (11)
��20 = [−sin (𝜃1 + 𝜃2) cos(𝜃1 + 𝜃2) 0
−cos (𝜃1 + 𝜃2) −sin(𝜃1 + 𝜃2) 0 0 0 0
] ∗ (��1 + ��2) (12)
��30 = [−sin (𝜃1 + 𝜃2 + 𝜃3) cos(𝜃1 + 𝜃2 + 𝜃3) 0
−cos (𝜃1 + 𝜃2 + 𝜃3) −sin(𝜃1 + 𝜃2 + 𝜃3) 0 0 0 0
] ∗ (��1 + ��2 + ��3) (13)
The angular velocity and acceleration of each body is calculated by the following equations (14 and 15).
��𝑘 = {��}𝑇[𝜔𝑘]{��} (14)
��𝑘 = (({��}𝑇[𝜔𝑘] + {��}𝑇[𝜔𝑘])){��} (15)
Partial angular velocities and their respective time derivatives of each body is shown in the equations
below (16 and 17).
Body 1 Body 2 Body 3
Partial Angular Velocity [𝜔1] = [𝐼
03𝑥3
03𝑥3
] [𝜔2] = [𝐼
𝑆10
03𝑥3
] [𝜔3] = [𝐼
𝑆10
𝑆20] (16)
Time Derivative of
Partial Angular Velocity [��1] = [
03𝑥3
03𝑥3
03𝑥3
] [��2] = [
03𝑥3
��10
03𝑥3
] [��3] = [
03𝑥3
��10
��20
] (17)
𝑆20 = 𝑆21 ∗ 𝑆10 = [ cos(𝜃1 + 𝜃2) sin (𝜃1 + 𝜃2) 0
−sin (𝜃1 + 𝜃2) cos(𝜃1 + 𝜃2) 0 0 0 1
] (9)
𝑆30 = 𝑆32 ∗ 𝑆21 ∗ 𝑆10 = [ cos(𝜃1 + 𝜃2 + 𝜃3) sin (𝜃1 + 𝜃2 + 𝜃3) 0
−sin (𝜃1 + 𝜃2 + 𝜃3) cos(𝜃1 + 𝜃2 + 𝜃3) 0 0 0 1
] (10)
December 6, 2016
13 of 58
In order to determine the mass-center velocity acceleration of each segment, generalized coordinates
were utilized. See equation (18).
{𝑦}𝑇 = [0 0 ��1 0 0 (��1 + ��2) 0 0 (��1 + ��2 + ��3)] (18)
where the Gimball matrix is shown in equation (19).
[𝐼] = [0 0 00 0 00 0 1
] [𝑊] = [𝐼 𝐼 𝐼0 𝐼 𝐼0 0 𝐼
] (19)
The mass center velocity of the thigh, shank, and foot is calculated by use of equation (21).
�� = {��}𝑇[𝑤][𝑉]{��} (20)
Lastly, the associated mass center velocity for each body is calculated by the following formula
��𝑗 =𝑑
𝑑𝑡��𝑗 = ({��1}𝑇 + {��}𝑇[𝑉𝐽] + {𝑦}𝑇[��𝐽]){��} (21)
December 6, 2016
14 of 58
Solutions
The numerical results of forward dynamics are calculated via the Matlab program (Appendix 3), utilizing
all of the kinematics relationships listed above. The code requests a time input from the user, pulls data
from the Winter et al. data file, and outputs kinematics solutions as well as a figure visually displaying a
graphical image representation of the orientation of the leg segments at the specified moment in time.
See Figure 6 below.
Figure 6: Figure output of Matlab code graphically displays representation of system orientation at a
specified moment in time
SolidWorks
The computer-aided design (CAD) software, SolidWorks, was used to model a human leg with the same
parameters as those used in the Matlab computation of the kinematics. A prescribed motion path was
applied to the leg model in order to simulate the gait cycle using angular velocities at each joint over time.
This then allowed for collection of kinematics data. This data is presented in the results section. The
following figure is an image of the CAD model developed for the human right leg.
Figure 7: CAD model of a human right leg
December 6, 2016
15 of 58
EQUATIONS OF MOTION
The general Equation of Motion is defined by the following equation (21),
𝑎�� + 𝑏�� + 𝑐�� = 𝑓 (21)
where matrices [a], [b], [c], {f}, {��}, and {��} are defined in equations (22) through (29),
{��} = [0 0 ��1 0 0 ��2 0 0 ��3]
{��} = [0 0 𝜃1 0 0 𝜃2 0 0 𝜃3]
(22)
(23)
The [a] is calculated as with equation (24),
[𝑎] = ∑ 𝑚𝑘 [𝑉𝑤]𝑘[𝑉𝑤𝑘]𝑇 + ∑[𝑤𝑘] [𝐼𝑜𝑘][𝑤𝑘]𝑇 (24)
where mk is the mass of each body and [𝐼𝑜𝑘] is the moment inertia obtained from data presented in de
Leva’s paper.
[𝑉𝑤𝑘]3𝑛×3 = [𝑊]3𝑛×3[𝑉𝑘]3𝑛×3 (25)
The Gimball matrix [W] is demonstrated in equation (19). Using the [𝑉𝑤𝑘] obtained for [a], [b] is computed
as with the following equation (26),
[𝑏] = ∑ 𝑚𝑘 [𝑉𝑤]𝑘[𝑉��𝑘
]𝑇 + ∑[𝑤𝑘] [𝐼𝑜𝑘][��𝑘]𝑇 (26)
The [c] matrix is obtained from the following equation (27),
[𝑐] = ∑[𝑤𝑘] [Ω𝑥𝑜𝑘][𝐼𝑜𝑘][𝑤𝑘]𝑇 (27)
where
[Ω𝑥𝑜𝑘]=[S𝑜𝑘
] [S𝑘𝑜] (28)
Finally, the equation of motion is computed as,
[𝑓] = [𝑉𝑤𝑘]{𝑓𝑘} + ∑[𝑤𝑘] [𝑀𝑘] (29)
Where,
{𝑓1} = [0 − 𝑚1𝑔 0]𝑇
{𝑓2} = [0 − 𝑚2𝑔 0]𝑇
{𝑓3} = [0 − 𝑚3𝑔 0]𝑇
(30)
December 6, 2016
16 of 58
CONSTRAINTS
Prescribed Motion
Knee and ankle motion is well defined during a standard gait cycle, and can be related by x and y-
coordinates in a global coordinate system. See Chart 1 below for the Winter et al. data plotted for both
the knee and ankle x and y-positions for one complete gait cycle,
Chart 1: X and Y position data for the right leg knee and ankle for one complete gait cycle, indicating
position locations of ‘Toe Off’ to ‘Heel Strike’.
As you can see, the knee is further advance in the x-direction at the time of toe-off. This explains the
forward-shifted knee positioning chart as compared to the heel position data. See graphical image below
for a rendering of the Matlab positioning output figure overlaid onto the same chart as above.
December 6, 2016
17 of 58
Figure 8: Matlab positioning output overlaid on Winter x-y gait cycle position data as a visual
representation.
The Winter et al. data for joint angular velocities is inputted into the SolidWorks software model to
simulate the motion path of the leg.
If one were to constrain a body to behave as a human leg during gait, an equation relating the x and y
position coordinates for the knee and ankle in Chart 1 would be required. However, it is not possible to
derive a relationship without breaking the data into small sections or over-simplifying the motion. As an
example proof of concept for constraining the system with prescribed motion, a three-body system is
constrained both by a constant speed and a straight line path. See Appendix 4 for the computational
Matlab code.
Prescribed Motion – Constant Speed and Straight Line
The equations for a constrained multibody system are given by:
𝑓∗ + 𝑓 + 𝐵𝑇𝜆 = 0 (31)
Then the T matrix is resolved in a way that constraint forces are equal to zero:
December 6, 2016
18 of 58
𝑇𝑓∗ + 𝑇𝑓 = 0
��1 + ��2 + ��3 = 0𝑟𝑎𝑑
𝑠𝑒𝑐
(32)
(33)
We could define the following constraint for an end effector:
𝑦1𝑐 = 2𝑡 (𝑐𝑜𝑛𝑠𝑡𝑎𝑛𝑡 𝑠𝑝𝑒𝑒𝑑)
𝑥13 = 0.5 (𝑓𝑜𝑙𝑙𝑜𝑤 𝑎 𝑠𝑡𝑟𝑎𝑖𝑔ℎ𝑡 𝑙𝑖𝑛𝑒)
(34)
(35)
Differentiation of the above yields:
or
Where:
Initial Condition:
According to the figure below our initial conditions will be:
December 6, 2016
19 of 58
INVERSE DYNAMICS
Free Body Diagram
Using the free body diagram (FBD) shown below in Figure 9, internal forces of the human leg can be
calculated.
Figure 9: Free body diagram of the human right leg, derived from the previously defined system. Green
arrows indicate joint moments, purple arrows indicate external forces, and blue arrows identify internal
forces between joints. The global coordinate system (O) is shown in yellow.
R1
R1
R2
R2
R2
R2
R3
R3
R3
R3
𝑛ሬ 1
𝑛ሬ 2
𝑛ሬ 3
O
Body 1
I1 m1
m1
g
Mhip
Mknee
I2 m2
Body
2
m2g
Mknee
Mankle
Mankle
Body
3 I3
m
3
m3
g
Fy,ground
Fx,ground
December 6, 2016
20 of 58
Many values listed in Figure 9 are already known about the system. The angular accelerations can be
pulled from the Winter et al. data, as well as the angles at each joint. Linear acceleration values in the x
and y direction of the center of masses were found during kinematics calculations. The mass of the
segments, distance from the joints to the center of masses, and moments of inertia about the mass
centers are found using scaling of the 50th percentile male anthropometric data. This is essential for all
inverse dynamics calculations.
Internal forces form equal and opposite reaction forces. Therefore, when one is solved for, the other can
be found. For example, the internal forces applied by the shank on the knee are equal and opposite to
those applied by the thigh on the knee. This is important because it easily reduces the number of unknown
values.
The two Fground forces in Figure 9 are ground reaction forces. In order to measure the force exerted by the
body on an external body or load, we need a suitable force-measuring device. Such a device, called a force
transducer, gives an electrical signal proportional to the applied force. Ground reaction forces, acting on
a foot during standing, walking or running, are traditionally measured by force plates. Force plate output
data provides us with ground reaction force vector components: vertical load plus two shear loads acting
along the force plate surface, that are usually resolved into anterior – posterior and medial – lateral
directions. The following figure shows a schematic of a force plate.
Figure 10: Traditional force plate schematic, used to measure ground reaction forces during gait.
The following figure represents an example of the characteristic curve for the ground reaction force in the
y-direction during the stance phase of the gait cycle. In modelling of the right leg, no ground reaction force
is present during the swing phase of the gait cycle.
December 6, 2016
21 of 58
Figure 11: Ground reaction force during the stance phase of the gait cycle, normalized as a percentage
of body weight
The highest ground reaction forces in the vertical direction occur during heel strike and just before toe
off, as indicated in Figure 11. These are the points of highest pressure, due to lowest surface area. During
heel strike, only the heel is supporting the body, and just before toe off, the distal end of the foot is
supporting the body. During both of these instances, the ground reaction force exceeds the total body
weight.
Please note that the ground reaction forces change as a function of the position of the foot during the gait
cycle, meaning that the Fground force locations in the FBD are arbitrary until the portion of the gait cycle of
interest is determined.
SIMM
The ground reaction force data used for this project was derived from the SIMM software, having applied
the same parameters and gait data as the previously completed kinematics of the system. SIMM is a
biomechanics simulation software tool. This software was utilized for this project for the creation of a gait
analysis test. This was then used for the measurement of ground reaction forces during typical gait. The
software allows for parameter inputs, such as body weight and walking speed. The reaction forces
measured by SIMM software are nothing more than the algebraic summation of all body segments mass-
acceleration products. See Figure 12 below for a graphical representation of the output of the SIMM
program for ground reaction force measurement, from heel contact to toe off.
December 6, 2016
22 of 58
Figure 12: SIMM model of ground force reaction measurement with a force plate. Green arrows show
direction and magnitude of the ground reaction force on the leg.
The following are the SIMM outputs for the ground reaction forces in the x, y, and z-directions from the
force plate measurement. Vertical lines represent gait cycle landmarks.
Figure 13: SIMM Ground Reaction Force output in the x-direction
December 6, 2016
23 of 58
Figure 14: SIMM Ground Reaction Force output in the y-direction
Figure 14: SIMM Ground Reaction Force output in the z-direction
December 6, 2016
24 of 58
Figure 16: SIMM Ground Reaction Force output in the x, y, and z-directions. The blue line represents the
vertical ground reaction force, and the red and green represent shear forces in the x and y directions
respectively.
December 6, 2016
25 of 58
Reaction Forces at Ankle
Internal forces and the moment at the ankle can be found using the following free body diagram:
Figure 17: Free body diagram of the human foot from Figure 9. Orange arrows are added to indicate
linear and angular accelerations.
Based on the FBD of the foot and Newton’s Second Law (𝐹 = 𝑚 ∗ 𝑎) and known values found previously,
values for the reaction forces (R) can be found. First, for the knee, reaction forces R2x and R2y can be
calculated. The reaction force at the ankle in the x-direction is as follows:
∑ 𝐹𝑥 = 𝑚𝑎𝑥
𝑅3𝑥 + 𝐹𝑥,𝑔𝑟𝑜𝑢𝑛𝑑 = 𝑚3𝑎3𝑥
The reaction force at the ankle in the y-direction is as follows:
∑ 𝐹𝑦 = 𝑚𝑎𝑦
𝑅3𝑦 + 𝐹𝑦,𝑔𝑟𝑜𝑢𝑛𝑑 − 𝑚3𝑔 = 𝑚3𝑎3𝑦
The moment about the ankle, Mankle can be also be found using the FBD above. The following is the
calculation for the angle θ in Figure 17:
𝜃 = 180 − 𝜃ℎ𝑖𝑝 + 𝜃𝑘𝑛𝑒𝑒 + 𝜃𝑎𝑛𝑘𝑙𝑒
The moment about the ankle, Mankle can be also be found using the following equation for sum of moments
at a joint. The term lCOM is defined as the distance to the center of mass of the foot from the point of action
of the force within its mathematical argument:
∑ 𝑀 = 𝐼𝛼
𝑀𝑎𝑛𝑘𝑙𝑒 + 𝐹𝑦,𝑔𝑟𝑜𝑢𝑛𝑑 × 𝑙𝐶𝑂𝑀 + 𝐹𝑥,𝑔𝑟𝑜𝑢𝑛𝑑 × 𝑙𝐶𝑂𝑀 − 𝑅3𝑦 × 𝑙𝐶𝑂𝑀 ∗ cos(θ) − 𝑅3𝑥 × 𝑙𝐶𝑂𝑀 ∗ sin(θ) = 𝐼3𝛼3
R3
R3
Body 3 I3
m
3
m3
g
Mankle
Fy,ground
Fx,ground
a3
a3
α3
December 6, 2016
26 of 58
Reaction Forces at the Knee
Internal forces and the moment at the knee can be found using the following free body diagram:
Figure 18: Free body diagram of the human leg shank from Figure 9. Orange arrows are added to
indicate linear and angular accelerations.
Based on the FBD of the shank and Newton’s Second Law (𝐹 = 𝑚 ∗ 𝑎) and known values found previously,
values for the reaction forces (R) can be found. For the knee, reaction forces R2x and R2y can be calculated.
The reaction force at the knee in the x-direction is as follows:
∑ 𝐹𝑥 = 𝑚𝑎𝑥
𝑅2𝑥 = 𝑚2𝑎2𝑥
The reaction force at the knee in the y-direction is as follows:
∑ 𝐹𝑦 = 𝑚𝑎𝑦
𝑅2𝑦 = 𝑚2𝑎2𝑦
The moment about the knee, Mknee can be also be found using the FBD above. The following is the
calculation for the angle θ in Figure 18:
θ = 90° − θℎ𝑖𝑝 − θ𝑘𝑛𝑒𝑒
Body 2
R2
R2
R3
R3
I2
m2
m2g
Mknee
Mankle
a2
a2
α2
θ
December 6, 2016
27 of 58
The general equation for sum of moments at a joint can be used to calculate Mknee about the center of
mass. The term lCOM is defined as the distance to the center of mass of the foot from the point of action
of the force within its mathematical argument:
∑ 𝑀 = 𝐼𝛼
𝑀𝑘𝑛𝑒𝑒 − 𝑅𝑥2 × 𝑙𝐶𝑂𝑀 ∗ sin(θ) − 𝑅𝑦2 × 𝑙𝐶𝑂𝑀 ∗ cos(θ) = 𝐼2𝛼2
Solutions
In order to solve for the joint internal forces and moments, it is required to know, at any given moment
during the gait cycle, the COP. SIMM provides position vector outputs for the gait cycle, calculated using
parameters listed in Figure 10 from the force plate, to meet this need. See Figures 19, 20, and 21 below:
Figure 19: x-position of the foot, measured by the force plate
Figure 20: y-position of the foot, measured by the force plate
December 6, 2016
28 of 58
Figure 21: z-position of the foot, measured by the force plate. From the previous Figure 10, it is evident
that the z-position does not change throughout gait (z is about equal to 0.6 m)
With data exported for each of these position values, the COP can be found. The COP is then used to
determine the moment arm for Fx,ground and Fy,ground. Using known values from previously completed
calculations for the system and the FBD shown in Figures 17 and 18, the reaction forces can be solved for.
However, data is not available for these charts, and the COP cannot be determined with the current
software. Further, a literature search did not yield any data sets for COP during the gait cycle.
A sample Matlab code was written as if data were available for the position of the COP over time in order
to calculate the reaction forces with values calculated in the kinematics and equations of motion Matlab
code in Appendix 3. See sample Matlab code in Appendix 5.
Expected outputs for the Matlab code for the reaction forces and joint moments over the gait cycle are
as follows:
December 6, 2016
29 of 58
Table 1: Ground Reaction Forces and Joint Moments over the gait cycle from Winter et al. Data
Parameter Ground Reaction Forces
Winter et al Data, Matlab Algorithm
Ankle
Knee
-150
-50
50
150
250
350
450
550
650
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Forc
e (N
ewto
ns)
Time (Seconds)
X Direction Y Direction
-100
0
100
200
300
400
500
600
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Forc
es (
New
ton
s)
Time (Seconds)
X Direction Y Direction
December 6, 2016
30 of 58
Parameter Joint Moments
Winter et al Data, Matlab Algorithm
Ankle
Knee
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
10
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Mo
men
t (N
m)
Time (Seconds)
-35
-25
-15
-5
5
15
25
35
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Mo
men
t (N
m)
Time (Seconds)
December 6, 2016
31 of 58
PROSTHETICS IMPLEMENTATION
STRESS ANALYSIS (FEA)
In order to apply the results of the gait analyses to a prosthetic limb, ANSYS can be used to optimize
prosthetic leg design. Using values calculated in the previous sections, a proof-of-concept ANSYS model
of a prosthetic shank was completed to complete a worst-case scenario stress analysis.
The following figure shows an example of an individual with a transfemoral prosthetic leg. The prosthetic
leg model was created in the CAD software, SolidWorks. The shank component will be assessed for
stresses by finite element analysis.
Figure 22: Multibody system showing a human with prosthetic leg.
December 6, 2016
32 of 58
Using the finite element analysis (FEA) software, ANSYS, a stress analysis of the shank of a prosthetic shank
was completed with force information found previously for the human leg. First, the model is uploaded
to the software.The material for this part of prosthetic leg is Titanium.
Figure 23: prosthetic leg shank model uploaded from SolidWorks to ANSYS
Then a finite element mesh was created for the model shank.
Figure 24: Prosthetic leg shank with a finite element mesh applied
December 6, 2016
33 of 58
Then, forces and moments at the joints, calculated previously in the inverse dynamics section, are
assigned and applied to the model within the ANSYS software.
Figure 25: Prosthetic leg shank model with forces and moments simulated at the joints.
Following application of forces, the deformation can be calculated. See Figure XX below.
Figure 26: ANSYS Model of Prosthetic Shank representing deformation in the body
Additionally, stresses in the body can be calculated.
December 6, 2016
34 of 58
Figure 27: ANSYS Model of Prosthetic Shank representing stresses with forces applied in the body
Figure 28: ANSYS Model of Prosthetic Shank including a factor of safety.
Such an analysis can be completed for any segment of a prosthetic leg, either transfemoral or transtibial.
December 6, 2016
35 of 58
RESULTS
The following section outlines results from various means for different aspects of the project.
Kinematics
Table 2: Angular Velocities and Accelerations for the Thigh, Shank, and Foot, resolved by the Matlab algorithm in Appendix 3.
Parameter
Angular Velocities Angular Accelerations
Matlab Algorithm
Thigh
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
0 0.05 0.1 0.15 0.2
An
gula
rV
elo
city
(rad
/s)
Time(s)-40
-30
-20
-10
0
10
20
30
0 0.05 0.1 0.15 0.2
An
gula
rA
cce
lera
tio
n(r
ad/s
^2)
Time(s)
December 6, 2016
36 of 58
Shank
Foot
0
1
2
3
4
5
6
7
8
0 0.05 0.1 0.15 0.2
An
gula
rV
elo
city
(rad
/s)
Time(s)-30
-10
10
30
50
70
90
0 0.05 0.1 0.15 0.2
An
gula
rA
cce
lera
tio
n(r
ad/s
^2)
Time(s)
-5
0
5
10
15
0 0.05 0.1 0.15 0.2An
gula
rV
elo
city
(rad
/s)
Time(s) -30
20
70
120
170
220
0 0.05 0.1 0.15 0.2
An
gula
rA
cce
lera
tio
n(r
ad/s
^2)
Time(s)
December 6, 2016
37 of 58
Table 3: Mass Center Velocities for the Thigh, Shank, and Foot, resolved by the Matlab algorithm in Appendix 3 as well as with SolidWorks
Parameter Computation #1 Compared to #2
Mass Center
Velocities Matlab Algorithm SolidWorks
Thigh (X)
Thigh (Y)
-0.9
-0.7
-0.5
-0.3
-0.1
0.1
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Ve
loci
ty(m
/s)
Time(s)
-0.8
-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Ve
loci
ty(m
/s)
Time(s)
December 6, 2016
38 of 58
Shank (X)
Shank (Y)
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Ve
loci
ty(r
ad/s
)
Tiem(s)
-2.5
-2
-1.5
-1
-0.5
0
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Ve
loci
ty(m
/s)
Time(s)
December 6, 2016
39 of 58
Foot (X)
Foot (Y)
-5
-4
-3
-2
-1
0
1
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(rad
/s)
Time(s)
-3
-2.5
-2
-1.5
-1
-0.5
0
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Ve
loci
ty(m
/s)
Time(s)
December 6, 2016
40 of 58
Table 4: Mass Center Accelerations for the Thigh, Shank, and Foot, resolved by the Matlab algorithm in Appendix 3 as well as with SolidWorks
Parameter Computation #1 Compared to #2
Mass Center
Accelerations Matlab Algorithm SolidWorks
Thigh (X)
Thigh (Y)
-6
-4
-2
0
2
4
6
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(s)
-4
-3
-2
-1
0
1
2
3
4
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(s)
December 6, 2016
41 of 58
Shank (X)
Shank (Y)
-12
-7
-2
3
8
13
0 0.05 0.1 0.15 0.2Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(s)
-12
-10
-8
-6
-4
-2
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(s)
December 6, 2016
42 of 58
Foot (X)
Foot (Y)
-10
-5
0
5
10
15
20
25
30
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(S)
-20
-15
-10
-5
0
5
10
0 0.05 0.1 0.15 0.2
Mas
sC
en
ter
Acc
ele
rati
on
(m/s
^2)
Time(s)
December 6, 2016
43 of 58
Results Discussion
Some possible reasons for discrepancies between the Matlab outputs, SolidWorks outputs, and Winter et
al. data are outlined below:
Our model takes anatomic geometries into account, whereas the Winter et al. data assumes cylindrical bodies for the leg segments.
There are differences in how the local and global coordinate systems are defined and utilized between the SolidWorks computations and those in the Matlab and Winter et al. data.
Our model includes a simplified version of the foot used in the Winter et al. dataset.
The muscular constraints were not included in either our model or the Winter et al. dataset.
The SolidWorks model is 3-dimensional, whereas the Matlab model or the Winter et al. dataset are both 2-dimensional.
Further analysis would provide better continuity between models and would likely result in more
accurate outputs.
December 6, 2016
44 of 58
CONCLUSION
We completed our dynamic analysis of a human leg via both theoretical approach and a more precise and
practical approach from CAD simulation. In the theoretical analysis, Amanda and Jinyi adapted Matlab
code from the midterm project to the new system to perform the forward dynamics, obtained results for
angular velocity, angular acceleration, as well as mass center velocity and acceleration. Furthermore, a
new Matlab program was developed for inverse dynamics focusing on ground reaction force and reaction
force at the knee. Plots of such measurements were generated to have a visual comparison with the CAD
simulation. For the more practical approach, Iman created the CAD model and completed various
simulation to compare with our Matlab simulation results. A 3D model of the human leg was created in
SolidWorks, allowing us to collect the dynamic measurements to compare with the Matlab outputs. We
also compared our inverse dynamic results with the CAD software simulation. We used SIMM, which is a
biomechanics simulation software, for a gait analysis test. We obtain ground reaction force throughout
the gait cycle and center of pressure vs. time. Based off data from SIMM, we were able to conduct a FEA
in ANSYS to examine the reaction forces at the knee. Lastly, we used Quintic Biomechanics’ sample data
for walking gait cycle on the treadmill as a proof of concept for potential future data collection. This could
also be used to measure the gait cycle of an individual with a lower limb prosthetic. This would make
computations for forward and inverse dynamics possible for the prosthetic scenario that we were unable
to accomplish without such data. These motion pictures were also used to prove our data for each
segment’s linear displacement, mass center velocity, as well as mass center acceleration.
We completed our objectives of analyzing the forward and inverse dynamics of a human leg throughout
a gait cycle via Matlab and modelling software simulations. We consolidated our understanding on
forward dynamics and inverse dynamics via Matlab programing, and CAD simulation. We also learned to
use motion capture software, such as Quintic Biomechanics to prove our founding from Matlab and CAD
software.
Throughout the process, the biggest challenge we faced was to perfect the Matlab code for its simulation
to match the real leg motion completed by the CAD software. The Matlab code only performs a perfect
planner motion and neglects joint friction. A human leg’s motion during a gait cycle is actually three-
dimensional and joint friction as well as other noises would change the results from those that we
calculated.
Therefore, to improve our project we need to work on a more precise model for Matlab simulation. To be
more specific, joint friction, muscle effects, and a third dimension of motion needed to be taken into
consideration. In addition, we did not discover any prosthetic gait cycle data. We wished to have such
information to understand the similarities and difference between a human leg and a prosthetic leg, which
would have allowed us to examine functionality discrepancies of an artificial leg, and understand how to
improve its design. For further future studies it might be good to use combination of Quintic Biomechanic
software with SIMM to found the actual forces and moments in each part of segments.
December 6, 2016
45 of 58
REFERENCES
1. Amirouche, Farid M. Fundamentals of multibody dynamics: theory and applications. Boston: Birkhauser, 2006. Print.
2. de Leva, P., 1996. Adjustments to Zatsiorsky–Seluyanov’s segment inertia parameters. J. Biomech. 29, 1223–1230.
3. Dumas, Raphael and Cheze, Laurence and Frossard, Laurent A. (2009) Loading applied on prosthetic knee of transfemoral amputee: comparison of inverse dynamics and direct measurements. Gait & Posture, 30(4). pp. 560-562.
4. L. Ren, R.K. Jones, D. Howard. Whole body inverse dynamics over a complete gait cycle based only on measured kinematics. J Biomech, 41 (12) (2008), pp. 2750–2759
5. S. Chowdhury, N. Kumar. Estimation of Forces and Moments of Lower Limb Joints from Kinematics Data and Inertial Properties of the Body by Using Inverse Dynamics Technique. Journal of Rehabilitation Robotics, pp. 93-98.
6. Winter, David A. Biomechanics and motor control of human movement. Hoboken, N.J: Wiley, 2009. Print.
December 6, 2016
46 of 58
Appendix 1
Kinematics Equations
Generalized Coordinates:
{𝑥} = [0 0 𝑥3 0 0 𝑥6 0 0 𝑥9] = [0 0 𝜃1 0 0 𝜃2 0 0 𝜃3]
{��} = [0 0 ��3 0 0 ��6 0 0 ��9] = [0 0 ��1 0 0 ��2 0 0 ��3]
Body Vectors
��1 = 𝜉 ≅ 0
Body 1 Body 2 Body 3
��2 = [𝑙1 0 0]𝑇 ��3 = [𝑙2 0 0]𝑇
��4 = [𝑙3 0 0]𝑇
𝑆𝑞2 = 𝑙1 [0 0 0
0 0 −10 1 0
] 𝑆𝑞3 = 𝑙2 [0 0 0
0 0 −10 1 0
] 𝑆𝑞4 = 𝑙3 [0 0 0
0 0 −10 1 0
]
𝑟1 = [𝑙1
20 0]
𝑇
𝑟2 = [𝑙2
20 0]
𝑇
𝑟3 = [𝑙3
20 0]
𝑇
𝑆𝑟1 =𝑙1
2[
0 0 00 0 −10 1 0
] 𝑆𝑟2 =𝑙2
2[
0 0 00 0 −10 1 0
] 𝑆𝑟3 =𝑙3
2[
0 0 00 0 −10 1 0
]
December 6, 2016
47 of 58
Transformation Matrices:
𝑆10 = [ cos(𝜃1) sin (𝜃1) 0
−sin (𝜃1) cos(𝜃1) 0 0 0 1
]
𝑆21 = [ cos(𝜃2) sin (𝜃2) 0
−sin (𝜃2) cos(𝜃2) 0 0 0 1
]
𝑆32 = [ cos(𝜃3) sin (𝜃3) 0
−sin (𝜃3) cos(𝜃3) 0 0 0 1
]
𝑆20 = 𝑆21 ∗ 𝑆10
𝑆20 = [ cos(𝜃1 + 𝜃2) sin (𝜃1 + 𝜃2) 0
−sin (𝜃1 + 𝜃2) cos(𝜃1 + 𝜃2) 0 0 0 1
]
𝑆30 = 𝑆32 ∗ 𝑆21 ∗ 𝑆10
𝑆30 = [ cos(𝜃1 + 𝜃2 + 𝜃3) sin (𝜃1 + 𝜃2 + 𝜃3) 0
−sin (𝜃1 + 𝜃2 + 𝜃3) cos(𝜃1 + 𝜃2 + 𝜃3) 0 0 0 1
]
Time Derivative of Transformation Matrices
��10 = [−sin (𝜃1) cos(𝜃1) 0
−cos (𝜃1) −sin(𝜃1) 0 0 0 0
] ∗ ��1
��20 = [−sin (𝜃1 + 𝜃2) cos(𝜃1 + 𝜃2) 0
−cos (𝜃1 + 𝜃2) −sin(𝜃1 + 𝜃2) 0 0 0 0
] ∗ (��1 + ��2)
��30 = [−sin (𝜃1 + 𝜃2 + 𝜃3) cos(𝜃1 + 𝜃2 + 𝜃3) 0
−cos (𝜃1 + 𝜃2 + 𝜃3) −sin(𝜃1 + 𝜃2 + 𝜃3) 0 0 0 0
] ∗ (��1 + ��2 + ��3)
December 6, 2016
48 of 58
Partial Angular Velocity Matrices Time Derivatives of Partial Angular Velocity
Matrices
I is a 3X3 identity matrix
[𝜔1] = [𝐼
03𝑥3
03𝑥3
] [��1] = [
03𝑥3
03𝑥3
03𝑥3
]
[𝜔2] = [𝐼
𝑆10
03𝑥3
] [��2] = [
03𝑥3
��10
03𝑥3
]
[𝜔3] = [𝐼
𝑆10
𝑆20] [��3] = [
03𝑥3
��10
��20
]
Angular Velocity Skew Matrices
[Ω10] = [��10][𝑆01] = ��1 [ 0 1 0−1 0 0 0 0 0
] [Ω𝑥01] = [Ω10]𝑇
[Ω20] = [��20][𝑆02] = (��1 + ��2) [ 0 1 0−1 0 0 0 0 0
] [Ω𝑥02] = [Ω20]𝑇
[Ω30] = [��30][𝑆03] = (��1 + ��2 + ��3) [ 0 1 0−1 0 0 0 0 0
] [Ω𝑥03] = [Ω30]𝑇
Gimball Matrix
[𝐼] = [0 0 00 0 00 0 1
] [𝑊] = [𝐼 𝐼 𝐼0 𝐼 𝐼0 0 𝐼
]
{𝑦}𝑇 = [0 0 ��1 0 0 (��1 + ��2) 0 0 (��1 + ��2 + ��3)]
December 6, 2016
49 of 58
Partial Velocity Matrices Time Derivative of Partial Velocity Matrices
[𝑉1] = [
[𝑆𝑟1] [𝑆10]
[03𝑥3][03𝑥3]
[03𝑥3]
[03𝑥3]] ��1 = [
[𝑆𝑟1] [𝜔ሬሬ 1] [𝑆10]
[03𝑥3] [03𝑥3] [03𝑥3]
[03𝑥3] [03𝑥3] [03𝑥3]]
[𝑉2] = [
[𝑆𝑞2] [𝑆10]
[𝑆𝑟2]
[03𝑥3][𝑆20][03𝑥3]
] ��2 = [
[𝑆𝑞2] [𝜔ሬሬ 1] [𝑆10]
[𝑆𝑟2] [𝜔ሬሬ 2] [𝑆20][03𝑥3] [03𝑥3] [03𝑥3]
]
[𝑉3] = [
[𝑆𝑞2] [𝑆10]
[𝑆𝑎3][𝑆𝑟3]
[𝑆20]
[𝑆30]
] ��3 = [
[𝑆𝑞2] [𝜔ሬሬ 1] [𝑆10]
[𝑆𝑞3] [𝜔ሬሬ 2] [𝑆20]
[𝑆𝑟3] [𝜔ሬሬ 3] [𝑆30]
]
December 6, 2016
50 of 58
Appendix 2
Equations of Motion
[𝑉𝑤1] = [𝑊][𝑉1]
[𝑉𝑤2] = [𝑊][𝑉2]
[𝑉𝑤3] = [𝑊][𝑉3]
[��𝑤1] = [𝑊][��1]
[��𝑤2] = [𝑊][��2]
[��𝑤3] = [𝑊][��3]
[𝐼10] = [𝑆01][𝐼1][𝑆10]
[𝐼20] = [𝑆02][𝐼2][𝑆20]
[𝐼30] = [𝑆03][𝐼3][𝑆30]
[𝐼1] = [0
0𝐼13
]
[𝐼2] = [0
0𝐼23
]
[𝐼3] = [0
0𝐼33
]
𝐼13, 𝐼23, and 𝐼33 are all calculated by use of the SolidWorks model
Gravity Forces on Bodies
{𝑓1} = [0 −𝑚1𝑔 0]𝑇
{𝑓2} = [0 −𝑚2𝑔 0]𝑇
{𝑓3} = [0 −𝑚3𝑔 0]𝑇
Gimball Matrix
[𝐼] = [0 0 00 0 00 0 1
] [𝑊] = [𝐼 𝐼 𝐼0 𝐼 𝐼0 0 𝐼
]
December 6, 2016
51 of 58
Appendix 3
Matlab Code – Kinematics and Equations of Motion
% Final - Numeric Solution for Forward Dynamics % clear all clc syms n1 n2 n3 t g = 9.8; %% Ask User for Lengths and Masses of Bodies in the System prompt = {'Lengths of Body 1, 2, and 3 with spaces between (mm)','Masses of Body 1, 2, and 3 with spaces between
(kg)'}; dlg_title = 'System Parameters'; num_lines = 1; defaultans = {'422.2 434 258.1','10.7894 3.5113 0.9417'}; ans1 = inputdlg(prompt,dlg_title,num_lines,defaultans); l = str2num(ans1{1,:}); m = str2num(ans1{2,:}); l1 = l(1)/1000; l2 = l(2)/1000; l3 = l(3)/1000; m1 = m(1); m2 = m(2); m3 = m(3); %% Ask User for Time Point of Interest prompt = {'Provide Time Point of Interest (seconds)'}; dlg_title = 'System Parameters'; num_lines = 1; defaultans = {'1'}; ans3 = inputdlg(prompt,dlg_title,num_lines,defaultans); t_sub = str2num(ans3{1,:}); %Choose time point %user input from other Matlab code %% Pull angular position, velocity, and acceleration info from Excel file data = xlsread('Winter_Appendix_data.xlsx','A3.LinearAngularKinematics'); %t_sub = 1.101; time = data(:,2) == t_sub; % Initial Angles th1_o = data(time,23)/57.2958; % Hip Joint (thigh) position at specific time th2_o = -data(time,13)/57.2958; % Knee Joint (shin) position at specific time th3_o = data(time,3)/57.2958; % Ankle Joint (foot) position at specific time % Angular Velocities th1_dot = data(time,24); % Hip Joint (thigh) velocity at specific time th2_dot = data(time,14); % Knee Joint (thigh) velocity at specific time
December 6, 2016
52 of 58
th3_dot = data(time,4); % Ankle Joint (thigh) velocity at specific time % Angular Accelerations th1_dotdot = data(time,25); % Hip Joint (thigh) acceleration at specific time th2_dotdot = data(time,15); % Knee Joint (thigh) acceleration at specific time th3_dotdot = data(time,5); % Ankle Joint (thigh) acceleration at specific time %% Plot the Visual Representation of the Initial Positioning and Size of Robot system = [0 0;... sin(th1_o-1.5708)*l1 -cos(th1_o-1.5708)*l1;... sin(th1_o-1.5708)*l1+sin(th1_o+th2_o-1.5708)*l2 -cos(th1_o-1.5708)*l1-cos(th1_o+th2_o-1.5708)*l2;... sin(th1_o-1.5708)*l1+sin(th1_o+th2_o-1.5708)*l2+sin(th1_o+th2_o+th3_o-1.5708)*l3 -cos(th1_o-1.5708)*l1-
cos(th1_o+th2_o-1.5708)*l2-cos(th1_o+th2_o+th3_o-1.5708)*l3]; figure (1); plot(system(:,1),system(:,2),'m-o','LineWidth',2,'MarkerSize',20,'MarkerEdgeColor','c') title('Position of Leg at Specified Moment in Time'); xlabel('Horizontal Position (meters)'); ylabel('Vertical Position (meters)'); axis([-1*(l1+l2+l3) l1+l2+l3 -1*(l1+l2+l3) 0]) %% Matrix Set-Up % n-Coordinate Matrix n = [n1; n2; n3]; % Transformation Matrices and their Time Derivatives S10 = vpa([cos(th1_dot*t+th1_o) sin(th1_dot*t+th1_o) 0; -sin(th1_dot*t+th1_o) cos(th1_dot*t+th1_o) 0; 0 0 1],3); S01 = vpa(transpose(S10),3); S21 = vpa([cos(th2_dot*t+th2_o) sin(th2_dot*t+th2_o) 0; -sin(th2_dot*t+th2_o) cos(th2_dot*t+th2_o) 0; 0 0 1],3); S20 = vpa(simplify(S21*S10),3); S02 = vpa(transpose(S20),3); S32 = vpa([cos(th3_dot*t+th3_o) sin(th3_dot*t+th3_o) 0; -sin(th3_dot*t+th3_o) cos(th3_dot*t+th3_o) 0; 0 0 1],3); S30 = vpa(simplify(S32*S21*S10),3); S03 = vpa(transpose(S30),3); S10_dot = vpa(diff(S10,t),3); S20_dot = vpa(diff(S20,t),3); S30_dot = vpa(diff(S30,t),3); x_dotT = [0 0 th1_dot 0 0 th2_dot 0 0 th3_dot]; x_dotdotT = [0 0 th1_dotdot 0 0 th2_dotdot 0 0 th3_dotdot]; % Identity and Zero Matrices I = eye(3); zeroMat = [zeros(3)]; % Partial Angular Velociy Matrices and thier Time Derivatives omega1 = [I; zeroMat; zeroMat]; omega2 = [I; S10; zeroMat]; omega3 = [I; S10; S20]; omega1_dot = [zeroMat; zeroMat; zeroMat]; omega2_dot = diff(omega2,t); omega3_dot = diff(omega3,t);
December 6, 2016
53 of 58
%% Angular Velocities wrt Reference Coordinates fprintf('Angular Velocity of Body 1:\n'); omegaR1 = vpa(x_dotT*omega1*n,3) fprintf('\nAngular Velocity of Body 2:\n'); omegaR2 = vpa(x_dotT*omega2*n,3) fprintf('\nAngular Velocity of Body 3:\n'); omegaR3 = vpa(x_dotT*omega3*n,3) %% Angular Accelerations wrt Reference Coordinates fprintf('Angular Acceleration of Body 1:\n'); alphaR1 = vpa(x_dotdotT*omega1*n,3) fprintf('\nAngular Acceleration of Body 2:\n'); alphaR2 = vpa(x_dotdotT*omega2*n,3) fprintf('\nAngular Acceleration of Body 3:\n'); alphaR3 = vpa(x_dotdotT*omega3*n,3) %% Define Body Vectors q1 = 0; q2 = transpose([l1 0 0]); q3 = transpose([l2 0 0]); q4 = transpose([l3 0 0]); r1 = transpose([l1/2 0 0]); r2 = transpose([l2/2 0 0]); r3 = transpose([l3/2 0 0]); % Skew Matrices Sq2 = l1*[0 0 0; 0 0 -1; 0 1 0]; Sq3 = l2*[0 0 0; 0 0 -1; 0 1 0]; Sq4 = l3*[0 0 0; 0 0 -1; 0 1 0]; Sr1 = l1/2*[0 0 0; 0 0 -1; 0 1 0]; Sr2 = l2/2*[0 0 0; 0 0 -1; 0 1 0]; Sr3 = l3/2*[0 0 0; 0 0 -1; 0 1 0]; % Partial Velocity Matrices and thier Time Derivatives pV1 = vpa([Sr1*S10;zeroMat;zeroMat],3); pV2 = vpa([Sq2*S10;Sr2*S20;zeroMat],3); pV3 = vpa([Sq2*S10;Sq3*S20;Sr3*S30],3); pV1_dot = vpa(diff(pV1,t),3); pV2_dot = vpa(diff(pV2,t),3); pV3_dot = vpa(diff(pV3,t),3); % Skew Matrices skew10 = vpa(simplify(S10_dot*transpose(S10)),3); skew20 = vpa(simplify(S20_dot*transpose(S20)),3); skew30 = vpa(simplify(S30_dot*transpose(S30)),3); skew01x = vpa(transpose(skew10),3); skew02x = vpa(transpose(skew20),3); skew03x = vpa(transpose(skew30),3); % Transformation Matrix % 1 position corresponds with non-zero degrees of freedom Ihat1 = [0 0 0; 0 0 0; 0 0 1]; Ihat2 = [0 0 0; 0 0 0; 0 0 1];
December 6, 2016
54 of 58
Ihat3 = [0 0 0; 0 0 0; 0 0 1]; W = [Ihat1 Ihat1 Ihat1; zeroMat Ihat1 Ihat1; zeroMat zeroMat Ihat1]; % Generalized Speeds yT = x_dotT*W; yT_dot1 = subs(yT,th1_dot,th1_dotdot); yT_dot2 = subs(yT_dot1,th2_dot,th2_dotdot); yT_dot = subs(yT_dot2,th3_dot,th3_dotdot); %% Mass Center Velocities V1 = vpa(yT*pV1*n,3); fprintf('Mass Center Velocity of Body 1:\n'); V1_s = vpa(subs(V1,'t',t_sub),3) V2 = vpa(yT*pV2*n,3); fprintf('\nMass Center Velocity of Body 2:\n'); V2_s = vpa(subs(V2,'t',t_sub),3) V3 = vpa(yT*pV3*n,3); fprintf('\nMass Center Velocity of Body 3:\n'); V3_s = vpa(subs(V3,'t',t_sub),3) %% Mass Center Accelerations acc1 = vpa((yT_dot*pV1+yT*pV1_dot)*n,3); fprintf('Mass Center Acceleration of Body 1:\n'); acc1_s = vpa(subs(acc1,'t',t_sub),3) acc2 = vpa((yT_dot*pV2+yT*pV2_dot)*n,3); fprintf('\nMass Center Acceleration of Body 2:\n'); acc2_s = vpa(subs(acc2,'t',t_sub),3) acc3 = vpa((yT_dot*pV2+yT*pV3_dot)*n,3); fprintf('\nMass Center Acceleration of Body 3:\n'); acc3_s = vpa(subs(acc3,'t',t_sub),3) %Equations of Motion %Matrix Set-Up V1w = W*pV1; V1w_dot = diff(V1w,t); V2w = W*pV2; V2w_dot = diff(V2w,t); V3w= W*pV3; V3w_dot = diff(V3w,t); I13 = (1/12)*m1*l1^2; I23 = (1/12)*m2*l2^2; I33 = (1/12)*m3*l3^2; I1 = [0 0 0; 0 0 0; 0 0 I13]; I2 = [0 0 0; 0 0 0; 0 0 I23]; I3 = [0 0 0; 0 0 0; 0 0 I33]; I10 = S01*I1*S10; I20 = S02*I2*S20; I30 = S03*I3*S30; %a-matrix fprintf('A Matrix:\n\n');
December 6, 2016
55 of 58
a =
vpa(simplify(m1*V1w*transpose(V1w)+m2*V2w*transpose(V2w)+m3*V3w*transpose(V3w)+omega1*I10*trans
pose(omega1)+omega2*I20*transpose(omega2)+omega3*I30*transpose(omega3)),3); a_s = vpa(subs(a,'t',t_sub),3) % b-matrix fprintf('\nB Matrix:\n\n'); b =
vpa(simplify(m1*V1w*transpose(V1w_dot)+m2*V2w*transpose(V2w_dot)+m3*V3w*transpose(V3w_dot)+omeg
a1*I10*transpose(omega1_dot)+omega2*I20*transpose(omega2_dot)+omega3*I30*transpose(omega3_dot)),3); b_s = vpa(subs(b,'t',t_sub),3) % c-matrix fprintf('\nC Matrix:\n\n'); c =
vpa(omega1*skew01x*I10*transpose(omega1)+omega2*skew02x*I20*transpose(omega2)+omega3*skew03x*I30*
transpose(omega3),3); c_s = vpa(subs(c,'t',t_sub),3) % Generalized Active Force % No moment acting on system (M=0) f1 = transpose([0 -m1*g 0]); f2 = transpose([0 -m2*g 0]); f3 = transpose([0 -m3*g 0]); %fprintf('\nForce Matrix:\n\n'); f = vpa(V1w*f1+V2w*f2+V3w*f3,3); f_s = vpa(subs(f,'t',t_sub),3); %% Generalized Forces Equation f3and6 = vpa([a(3,3) a(3,6) a(3,9); a(6,3) a(6,6) a(6,9); a(9,3) a(9,6) a(9,9)]*[th1_dotdot; th2_dotdot;
th3_dotdot]+[b(3,3) b(3,6) b(3,9); b(6,3) b(6,6) b(6,9); b(9,3) b(9,6) b(9,9)]*[th1_dot; th2_dot; th3_dot]+[c(3,3)
c(3,6) c(3,9); c(6,3) c(6,6) c(6,9); c(9,3) c(9,6) c(9,9)]*[th1_dot; th2_dot; th3_dot],3); gf = vpa([f3and6(1); f3and6(2); f3and6(3)] == [a(3,3) a(3,6) a(3,9); a(6,3) a(6,6) a(6,9); a(9,3) a(9,6)
a(9,9)]*[th1_dotdot; th2_dotdot; th3_dotdot]+[b(3,3) b(3,6) b(3,9); b(6,3) b(6,6) b(6,9); b(9,3) b(9,6)
b(9,9)]*[th1_dot; th2_dot; th3_dot]+[c(3,3) c(3,6) c(3,9); c(6,3) c(6,6) c(6,9); c(9,3) c(9,6) c(9,9)]*[th1_dot;
th2_dot; th3_dot],3);
December 6, 2016
56 of 58
Appendix 4
Matlab Code - Constraints
c=(omegap1*transpose(Omega10)*I10*transpose(omegap1))+...
(omegap2*transpose(Omega20)*I20*transpose(omegap2))+(omegap3*transpose(Omega30)*I30*transpose(omegap3));
f1=[0;-m1*g;0];
f2=[0;-m2*g;0];
f3=[0;-m3*g;0];
f=VW1*f1+VW2*f2+VW3*f3;
a=subs(a,[dtheta1*t dtheta2*t dtheta3*t],[t1 t2 t3]);
b=subs(b,[dtheta1*t dtheta2*t dtheta3*t],[t1 t2 t3]);
c=subs(c,[dtheta1*t dtheta2*t dtheta3*t],[t1 t2 t3]);
f=subs(f,[dtheta1*t dtheta2*t dtheta3*t],[t1 t2 t3]);
a=subs(a,[dtheta1 dtheta2 dtheta3],[dt1 dt2 dt3]);
b=subs(b,[dtheta1 dtheta2 dtheta3],[dt1 dt2 dt3]);
c=subs(c,[dtheta1 dtheta2 dtheta3],[dt1 dt2 dt3]);
f=subs(f,[dtheta1 dtheta2 dtheta3],[dt1 dt2 dt3]);
a=simplify(a);
b=simplify(b);
c=simplify(c);
f=simplify(f);
a1p=subs(a,[l1 l2 l3 m1 m2 m3],[0.4 0.5 0.1 10 1.5 0.5]);
a2p=[a1p(3,3) a1p(3,6) a1p(3,9);a1p(6,3) a1p(6,6) a1p(6,9);a1p(9,3) a1p(9,6) a1p(9,9)];
b1p=subs(b,[l1 l2 l3 m1 m2 m3],[0.4 0.5 0.1 10 1.5 0.5]);
b2p=[b1p(3,3) b1p(3,6) b1p(3,9);b1p(6,3) b1p(6,6) b1p(6,9);b1p(9,3) b1p(9,6) b1p(9,9)];
f1p=subs(f,[l1 l2 l3 m1 m2 m3 g],[0.4 0.5 0.1 10 1.5 0.5 9.81]);
f2p=[f1p(3,1);f1p(6,1);f1p(9,1)];
B=[-l1*sin(t1)-l2*sin(t1+t2)-l3*sin(t1+t2+t3) -l2*sin(t1+t2)-l3*sin(t1+t2+t3) -
l3*sin(t1+t2+t3);l1*cos(t1)+l2*cos(t1+t2)+l3*cos(t1+t2+t3) +l2*cos(t1+t2)+l3*cos(t1+t2+t3) +l3*cos(t1+t2+t3)];
B=subs(B,[l1 l2 l3],[0.4 0.5 0.1]);
B=subs(B,[t1 dt1 t2 dt2 t3 dt3],[j1 dj1 j2 dj2 j3 dj3]);
Bdot=diff(B,t);
Bt=subs(B,[dj1*t dj2*t dj3*t],[j1 j2 j3]);
Bdott=subs(Bdot,[dj1*t dj2*t dj3*t],[j1 j2 j3]);
p=b2p*[dj1;dj2;dj3];
o=f2p-p;
e=Bdott*[dj1;dj2;dj3];
W1=[((sin(j1 + j2 + j3))^2/100 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 +
(2*sin(j1))/5)^2)^(1/2);0;0];
X1=Bt(1,:);
U1=(W1-transpose(X1))/(((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1
+ j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 +
(sin(j1 + j2 + j3))^2/100)^(1/2));
H1=eye(3,3)-2*U1*transpose(U1);
X2=H1*transpose(Bt);
s1=((cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2)^2 + (cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5)^2 + (cos(j1 + j2 + j3))^2/100)-
((cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5)^2);
sq1=sqrt(s1);
W2=[Bt(2,1);sq1;0];
U2=(W2-X2(:,2))/(((cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5 + ((2*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5
+ ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 +
j3)^2/100)^(1/2))^2)/((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 +
(2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 +
j3)^2/100)^(1/2))^2) - 1)*(cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5) + (2*(cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2)*(sin(j1
+ j2 + j3)/10 + sin(j1 + j2)/2)*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 +
(sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2)))/((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 +
sin(j1 + j2 + j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 +
j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2) + (cos(j1 + j2 + j3)*sin(j1 + j2 + j3)*(sin(j1 + j2
+ j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 +
(2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2)))/(50*((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100 + (sin(j1
+ j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 +
December 6, 2016
57 of 58
(2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2)))^2 + ((cos(j1 + j2 + j3)*(sin(j1 + j2 + j3)^2/(50*((sin(j1 + j2 + j3)/10 + sin(j1
+ j2)/2)^2 + sin(j1 + j2 + j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2
+ (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2)) - 1))/10 + (sin(j1 + j2 + j3)*(cos(j1
+ j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5)*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 +
j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2)))/(5*((sin(j1 + j2 + j3)/10 +
sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 +
j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2)) + (sin(j1 + j2 + j3)*(cos(j1
+ j2 + j3)/10 + cos(j1 + j2)/2)*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2))/(5*((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 +
j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1
+ j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2)))^2 + (((cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2)^2 + cos(j1 + j2
+ j3)^2/100)^(1/2) + ((2*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2)/((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100
+ (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 +
j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2) - 1)*(cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2) + (2*(sin(j1 + j2 + j3)/10
+ sin(j1 + j2)/2)*(cos(j1 + j2 + j3)/10 + cos(j1 + j2)/2 + (2*cos(j1))/5)*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1
+ j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2)))/((sin(j1
+ j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100 + (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 +
sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2) + (cos(j1 + j2
+ j3)*sin(j1 + j2 + j3)*(sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2))/(50*((sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2)^2 + sin(j1 + j2 + j3)^2/100
+ (sin(j1 + j2 + j3)/10 + sin(j1 + j2)/2 + (2*sin(j1))/5 + ((sin(j1 + j2)/2 + sin(j1 + j2 + j3)/10)^2 + (sin(j1 + j2)/2 + sin(j1 + j2 +
j3)/10 + (2*sin(j1))/5)^2 + sin(j1 + j2 + j3)^2/100)^(1/2))^2)))^2)^(1/2));
H2=eye(3,3)-2*U2*transpose(U2);
H=H2*H1;
T=[0,0,1]*H;
A=[T*a2p;Bt];
D=[T*o;-e];
r=inv(A)*D
diary ('myVariabler1.txt');
r(1,1)
diary off
diary ('myVariabler2.txt');
r(2,1)
diary off
diary ('myVariabler3.txt');
%r(3,1)
%diary off
December 6, 2016
58 of 58
Appendix 5
Matlab Code – Inverse Dynamics
data = xlsread('dynamics.xlsx','A.forward'); time = data(3:17,2) == t_sub; %counter-clockward is the positive reaction a_x_ankle = data(time,20); R_x_ankle= m_3*a_x_foot+R_x; %R_x_ankle is x component of the ankle reaction force %R_x is the x component of the ground reaction force a_y_ankle = data(time,21); %R_y_ankle is Y component of the ankle reaction force R_foot_y= m_3*a_y_foot+m_3*g+R_y; %R_y is the y component of the ground reaction force
M_ankle= M_1 + l_x_ground * R_x+l_y_ground - l_x_ankle * R_x_ankle - l_y_ankle * R_y_ankle %M_1 is the moment repect to the contact point with the ground %l_x_ground is x component of the moment arm respect ground contact point %l_y_ground is the y component of moment arm respect to ground contact point %l_x_ankle is x component of the moment arm respect to ankle %l_y_ankle is the y component of moment arm respect to ankle
a_x_shin = data(time,13); a_y_shin = data(time,14);
R_x_shin= m_2*a_x_shin+R_x_ankle; %R_x_shin is x component of the shin reaction force R_y_shin= m_2*a_y_shin+m_2*g+R_y_ankle; %R_y_shin is x component of the shin reaction force M_shin= M_ankle + l_x_ankle * R_x + l_y_ankle - l_x_shin * R_x_shin - l_y_shin * R_y_shin %l_x_shin is x component of the moment arm respect to ankle %l_y_shin is the y component of moment arm respect to ankle R_Knee= M_shin * l_knee %R_knee is the reaction force at the knee %l_knee is the moment arm respect to the center of pressure of the shin
