Description

IMU sensors, Accel Gyro

All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.

Related Documents

Share

Transcript

Estimation of IMU and MARG orientation using agradient descent algorithm
Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan
Abstract
—This paper presents a novel orientation algorithmdesigned to support a computationally efﬁcient, wearable inertialhuman motion tracking system for rehabilitation applications. Itis applicable to inertial measurement units (IMUs) consisting of tri-axis gyroscopes and accelerometers, and magnetic angularrate and gravity (MARG) sensor arrays that also include tri-axismagnetometers. The MARG implementation incorporates mag-netic distortion compensation. The algorithm uses a quaternionrepresentation, allowing accelerometer and magnetometer data tobe used in an analytically derived and optimised gradient descentalgorithm to compute the direction of the gyroscope measurementerror as a quaternion derivative. Performance has been evaluatedempirically using a commercially available orientation sensorand reference measurements of orientation obtained using anoptical measurement system. Performance was also benchmarkedagainst the propriety Kalman-based algorithm of orientationsensor. Results indicate the algorithm achieves levels of accuracymatching that of the Kalman based algorithm;
<
0
.
8
◦
staticRMS error,
<
1
.
7
◦
dynamic RMS error. The implications of thelow computational load and ability to operate at small samplingrates signiﬁcantly reduces the hardware and power necessaryfor wearable inertial movement tracking, enabling the creationof lightweight, inexpensive systems capable of functioning forextended periods of time.
I. I
NTRODUCTION
The accurate measurement of orientation plays a criticalrole in a range of ﬁelds including: aerospace [1], robotics [2],[3], navigation [4], [5] and human motion analysis [6], [7]and machine interaction [8]. In rehabilitation, motion trackingis vital enabling technology, in particular for monitoringoutside clinical environs; ideally, a patient’s activities couldbe continuously monitored, and subsequently corrected. Whileextensive work has been performed for motion tracking forrehabilitation, an unobtrusive, wearable system capable of logging data for extended periods of time has yet to berealized. Existing systems often require a laptop or handheldPC to be carried by the subject due to the processing, datastorage, and power requirements of the sensory equipment.This is not practical outside of a laboratory environment, thusdetailed data may only be obtained for short periods of timefor a limited range of subject’s motion. More precise datarepresentative of a subject’s natural behaviour over extendedperiods of time (e.g. an entire day or even a week) would be
Sebastian Madgwick is with the Department of Mechanical Engineering,University of Bristol, e-mail: s.madgwick@bristol.ac.uk.Ravi Vaidyanathan is with the Department of Mechanical Engineering, Uni-versity of Bristol, Queens Building, BS8 1TR and the Department of Sys-tems Engineering at the US Naval Postgraduate School, Monterey, CA, USA,93940. e-mail: r.vaidyanathan@bristol.ac.uk.Andrew Harrison is with the Department of Mechanical Engineering, Uni-versity of Bristol, e-mail: andrew.j.l.harrison@bristol.ac.uk.
of signiﬁcant utility in this realm. In a recent survey, Zhoua[7], cited real time operation, wireless properties, correctnessof data, and portability as major deﬁciencies that must beaddressed to realize a clinically viable system.
A. Inertial Motion Tracking Systems
Whilst a variety of technologies enable the measurement of orientation, inertial based sensory systems have the advantageof being completely self contained such that the measuremententity is constrained neither in motion nor to any speciﬁcenvironment or location. An IMU (Inertial Measurement Unit)consists of gyroscopes and accelerometers enabling the track-ing of rotational and translational movements. In order tomeasure in three dimensions, tri-axis sensors consisting of 3mutually orthogonal sensitive axes are required. A MARG(Magnetic, Angular Rate, and Gravity) sensor is a hybridIMU which incorporates a tri-axis magnetometer. An IMUalone can only measure an attitude relative to the direction of gravity which is sufﬁcient for many applications [2], [1], [6].MARG systems, also known as AHRS (Attitude and HeadingReference Systems) are able to provide a
complete
measure-ment of orientation relative to the direction of gravity and theearth’s magnetic ﬁeld. An orientation estimation algorithm isa fundamental component of any IMU or MARG system. It isrequired to fuse together the separate sensor data into a single,optimal estimate of orientation.The Kalman ﬁlter [9] has become the accepted basis forthe majority of orientation algorithms [2], [10], [11], [12]and commercial inertial orientation sensors; xsens [13], micro-strain [14], VectorNav [15], Intersense [16], PNI [17] andCrossbow [18] all produce systems founded on its use. Thewidespread use of Kalman-based solutions are a testamentto their accuracy and effectiveness, however, they have anumber of disadvantages. They can be complicated to im-plement which is reﬂected by the numerous solutions seenin the subject literature [2], [10], [11], [12], [19], [20], [21],[22], [23]. The linear regression iterations, fundamental to theKalman process, demand sampling rates which can far exceedthe subject bandwidth (e.g. a sampling rate between 512 Hz[13] and 30 kHz [14] may be necessary for human motioncapture applications where system portability is critical). Thestate relationships describing rotational kinematics in three-dimensions typically require large state vectors and an ex-tended Kalman ﬁlter implementation [2], [12], [19] to linearisethe problem.These challenges demand a large computational load forimplementation of Kalman-based solutions and provide a clear
2011 IEEE International Conference on Rehabilitation Robotics Rehab Week Zurich, ETH Zurich Science City, Switzerland, June 29 - July 1, 2011
978-1-4244-9861-1/11/$26.00 ©2011 IEEE 179
motivation for alternative approaches. Previous approachesto address these issues have implemented either fuzzy pro-cessing [1], [3] or frequency domain ﬁlters [24] to favouraccelerometer measurements of orientation at low angularvelocities and the integrated gyroscope measurements at highangular velocities. Such an approach is simple but may onlybe effective under limited operating conditions. Bachman
et al
[25] and Mahony
et al
[26] present separate algorithms whichboth employ a complementary ﬁlter process. This algorithmstructure has been shown to provide effective performance atrelatively little computational expense.This paper introduces orientation estimation algorithm thatis applicable to both IMU and MARG systems. The algorithmemploys a quaternion representation of orientation (as in:[25], [12], [19]) to describe the coupled nature of orientationsin three-dimensions and is not subject to the problematicsingularities associated with an Euler angle representation.A complete derivation and empirical evaluation of the newalgorithm is presented. Its performance is benchmarked againstexisting commercial ﬁlters and veriﬁed with optical trackingsystem.II. O
RGANISATION OF PAPER
Section III delineates the mathematical derivation of theorientation estimation algorithm, including a description of theparameterization and compensation for magnetic distortion.Section IV describes the experimental equipment used to testand verify the performance of the algorithm. Section V quanti-ﬁes the experimental testing and accuracy of the algorithm andcompares it to existing systems. Section VII expands brieﬂygives details on implementations of the system underwaycurrently in our laboratory in human motion tracking whileSection VI summarizes conclusions and contributions of thiswork. Throughout the paper, a notation system of leadingsuperscripts and subscripts adopted from Craig [27] is usedto denote the relative frames of orientations and vectors. Aleading subscript denotes the frame being described and aleading superscript denotes the frame this is with referenceto. For example, For example,
AB
ˆ
q
describes the orientation of frame
B
relative to frame
A
and
A
ˆ
v
is a vector described inframe
A
.III. A
LGORITHM DERIVATION
A. Orientation from angular rate
A tri-axis gyroscope will measure the angular rate aboutthe
x
,
y
and
z
axes of the senor frame, termed
ω
x
,
ω
y
and
ω
z
respectively. If these parameters (in rads
−
1
) are arrangedinto the vector
S
ω
deﬁned by equation (1), the quaternionderivative describing rate of change of the earth frame relativeto the sensor frame
S E
˙
q
can be calculated [28] as equation (2).The
⊗
operate denotes a quaternion product and the
ˆ
accentdenotes a normalised vector of unit length.
S
ω
=
0
ω
x
ω
y
ω
z
(1)
S E
˙
q
= 12
S E
ˆ
q
⊗
S
ω
(2)The orientation of the earth frame relative to the sensorframe at time
t
,
E S
q
ω,t
, can be computed by numericallyintegrating the quaternion derivative
S E
˙
q
ω,t
as described byequations (3) and (4), provided that initial conditions areknown. In these equations,
S
ω
t
is the angular rate measuredat time
t
,
∆
t
is the sampling period and
S E
ˆ
q
est,t
−
1
is theprevious estimate of orientation. The subscript
ω
indicates thatthe quaternion is calculated from angular rates.
S E
˙
q
ω,t
= 12
S E
ˆ
q
est,t
−
1
⊗
S
ω
t
(3)
S E
q
ω,t
=
S E
ˆ
q
est,t
−
1
+
S E
˙
q
ω,t
∆
t
(4)
B. Orientation from a homogenous ﬁeld
In the context of an orientation estimation algorithm, it willinitially be assumed that an accelerometer will measure onlygravity and a magnetometer will measure only the earth’smagnetic ﬁeld. If the direction of an earth’s ﬁeld is knownin the earth frame, a measurement of the ﬁeld’s directionwithin the sensor frame will allow an orientation of thesensor frame relative to the earth frame to be calculated.However, for any given measurement there will not be a uniquesensor orientation solution, instead there will inﬁnite solutionsrepresented by all those orientations achieved by the rotationthe true orientation around an axis parallel with the ﬁeld.A quaternion representation requires a single solution to befound. This may be achieved through the formulation of anoptimisation problem where an orientation of the sensor,
S E
ˆ
q
,is found as that which aligns a predeﬁned reference directionof the ﬁeld in the earth frame,
E
ˆ
d
, with the measured ﬁeldin the sensor frame,
S
ˆ
s
; thus solving (5) where equation (6)deﬁnes the objective function.
min
SE
ˆ
q
∈
4
f
(
S E
ˆ
q
,
E
ˆ
d
,
S
ˆ
s
)
(5)
f
(
S E
ˆ
q
,
E
ˆ
d
,
S
ˆ
s
) =
S E
ˆ
q
∗
⊗
E
ˆ
d
⊗
S E
ˆ
q
−
S
ˆ
s
(6)Many optimisation algorithms exist but the gradient descentalgorithm is one of the simplest to both implement andcompute. Equation (7) describes the gradient descent algorithmfor
n
iterations resulting in an orientation estimation of
S E
ˆ
q
n
+1
based on an ‘initial guess’ orientation
S E
ˆ
q
0
and a variablestep-size
µ
. Equation (8) computes an error direction on thesolution surface deﬁned by the objective function,
f
, and itsJacobian,
J
.
S E
q
k
+1
=
S E
ˆ
q
k
−
µ
∇
f
(
S E
ˆ
q
k
,
E
ˆ
d
,
S
ˆ
s
)
∇
f
(
S E
ˆ
q
k
,
E
ˆ
d
,
S
ˆ
s
)
, k
= 0
,
1
,
2
...n
(7)
∇
f
(
S E
ˆ
q
k
,
E
ˆ
d
,
S
ˆ
s
) =
J
T
(
S E
ˆ
q
k
,
E
ˆ
d
)
f
(
S E
ˆ
q
k
,
E
ˆ
d
,
S
ˆ
s
)
(8)Equations (7) and (8) describe the general form of thealgorithm applicable to a ﬁeld predeﬁned in any direction.However, if the reference direction of the ﬁeld is deﬁned toonly have components within 1 or 2 of the principle axis of
180
the earth coordinate frame then the equations simplify. Anappropriate convention would be to assume that the directionof gravity deﬁnes the vertical,
z
axis as shown in equation (10).Substituting
E
ˆ
g
and normalised accelerometer measurement
S
ˆ
a
for
E
ˆ
d
and
S
ˆ
s
respectively, yields the simpliﬁed objectivefunction and Jacobian deﬁned by equations (12) and (13).
S E
ˆ
q
=
q
1
q
2
q
3
q
4
(9)
E
ˆ
g
=
0 0 0 1
(10)
S
ˆ
a
=
0
a
x
a
y
a
z
(11)
f
g
(
S E
ˆ
q
,
S
ˆ
a
) =
2(
q
2
q
4
−
q
1
q
3
)
−
a
x
2(
q
1
q
2
+
q
3
q
4
)
−
a
y
2(
12
−
q
22
−
q
23
)
−
a
z
(12)
J
g
(
S E
ˆ
q
) =
−
2
q
3
2
q
4
−
2
q
1
2
q
2
2
q
2
2
q
1
2
q
4
2
q
3
0
−
4
q
2
−
4
q
3
0
(13)The earth’s magnetic ﬁeld can be considered to have com-ponents in one horizontal axis and the vertical axis; the verticalcomponent due to the inclination of the ﬁeld which is between
65
◦
and
70
◦
to the horizontal in the UK [29]. This can berepresented by equation (14). Substituting
E
ˆ
b
and normalisedmagnetometer measurement
S
ˆ
m
for
E
ˆ
d
and
S
ˆ
s
respectively,yields the simpliﬁed objective function and Jacobian deﬁnedby equations (16) and (17).
E
ˆ
b
=
0
b
x
0
b
z
(14)
S
ˆ
m
=
0
m
x
m
y
m
z
(15)
f
b
(
S E
ˆ
q
,
E
ˆ
b
,
S
ˆ
m
) =
2
b
x
(0
.
5
−
q
23
−
q
24
)+2
b
x
(
q
2
q
3
−
q
1
q
4
)+2
b
x
(
q
1
q
3
+
q
2
q
4
)+2
b
z
(
q
2
q
4
−
q
1
q
3
)
−
m
x
2
b
z
(
q
1
q
2
+
q
3
q
4
)
−
m
y
2
b
z
(0
.
5
−
q
22
−
q
23
)
−
m
z
(16)
J
b
(
S E
ˆ
q
,
E
ˆ
b
) =
−
2
b
z
q
3
2
b
z
q
4
−
2
b
x
q
4
+ 2
b
z
q
2
2
b
x
q
3
+ 2
b
z
q
1
2
b
x
q
3
2
b
x
q
4
−
4
b
z
q
2
−
4
b
x
q
3
−
2
b
z
q
1
−
4
b
x
q
4
+ 2
b
z
q
2
2
b
x
q
2
+ 2
b
z
q
4
−
2
b
x
q
1
+ 2
b
z
q
3
2
b
x
q
1
−
4
b
z
q
3
2
b
x
q
2
(17)As has already been discussed, the measurement of gravityor the earth’s magnetic ﬁeld alone will not provide a uniqueorientation of the sensor. To do so, the measurements and refer-ence directions of both ﬁelds may be combined as described byequations (18) and (19). Whereas the solution surface createdby the objective functions in equations (12) and (16) have aglobal minimum deﬁned by a line, the solution surface deﬁneby equation (18) has a minimum deﬁne by a single point,provided that
b
x
= 0
.
f
g,b
(
S E
ˆ
q
,
S
ˆ
a
,
E
ˆ
b
,
S
ˆ
m
) =
f
g
(
S E
ˆ
q
,
S
ˆ
a
)
f
b
(
S E
ˆ
q
,
E
ˆ
b
,
S
ˆ
m
)
(18)
J
g,b
(
S E
ˆ
q
,
E
ˆ
b
) =
J
T g
(
S E
ˆ
q
)
J
T b
(
S E
ˆ
q
,
E
ˆ
b
)
(19)A conventional approach to optimisation would requiremultiple iterations of equation (7) to be computed for each neworientation and corresponding senor measurements. However,it is acceptable to compute one iteration per time sampleprovided that the convergence rate of the estimated orientationgoverned by
µ
t
is equal or greater than the rate of changeof physical orientation. Equation (20) calculates the estimatedorientation
S E
q
∇
,t
computed at time
t
based on a previousestimate of orientation
S E
ˆ
q
est,t
−
1
and the objective functionerror
∇
f
deﬁned by sensor measurements
S
ˆ
a
t
and
S
ˆ
m
t
sampled at time
t
. The form of
∇
f
is chosen according tothe sensors in use, as shown in equation (21). The subscript
∇
indicates that the quaternion is calculated using the gradientdescent algorithm.
S E
q
∇
,t
=
S E
ˆ
q
est,t
−
1
−
µ
t
∇
f
∇
f
(20)
∇
f
=
J
T g
(
S E
ˆ
q
est,t
−
1
)
f
g
(
S E
ˆ
q
est,t
−
1
,
S
ˆ
a
t
)
J
T g,b
(
S E
ˆ
q
est,t
−
1
,
E
ˆ
b
)
f
g,b
(
S E
ˆ
q
est,t
−
1
,
S
ˆ
a
t
,
E
ˆ
b
,
S
ˆ
m
t
)
(21)An appropriate value of
µ
t
is that which ensures theconvergence rate of
S E
q
∇
,t
is limited to the physical orientationrate as this avoids overshooting due an unnecessarily large stepsize. Therefore
µ
t
can be calculated as equation (22) where
∆
t
is the sampling period,
S E
˙
q
ω,t
is the rate of change of orientation measured by gyroscopes and
α
is an augmentationof
µ
to account for noise in accelerometer and magnetometermeasurements.
µ
t
=
α
S E
˙
q
ω,t
∆
t, α >
1
(22)
C. Algorithm fusion process
In practice,
S E
q
ω,t
may start from incorrect initial conditionsand acclimate errors due to gyroscope measurement noiseand
S E
q
∇
,t
will provide an incorrect estimate when the ac-celerometer is not stationary or the magnetometer exposed tointerferences. The goal of the fusion algorithm is to providean orientation estimate where
S E
q
ω,t
is used to ﬁlter out highfrequency errors in
S E
q
∇
,t
, and
S E
q
∇
,t
is used both to com-pensate for integral drift in
S E
q
ω,t
and to provide convergencefrom initial conditions.An estimated orientation of the earth frame relative to thesensor frame,
S E
q
est,t
, is obtained through the fusion of the twoseparate orientation calculations,
S E
q
ω,t
and
S E
q
∇
,t
as describedby equation (23) where
γ
t
and
(1
−
γ
t
)
are weights appliedto each orientation calculation.
S E
q
est,t
=
γ
tS E
q
∇
,t
+ (1
−
γ
t
)
S E
q
ω,t
,
0
≤
γ
t
≤
1
(23)
181
An optimal value of
γ
t
is therefore that which ensures theweighted rate of divergence of
S E
q
ω
due to integral drift isequal to the weighted rate of convergence of
S E
q
∇
. This isrepresented by equation (24) where
µ
t
∆
t
is the convergencerate of
S E
q
∇
and
β
is the divergence rate of
S E
q
ω
expressed asthe magnitude of a quaternion derivative corresponding to thegyroscope measurement error. Equation (24) can be rearrangedto deﬁne
γ
t
as equation (25).
(1
−
γ
t
)
β
=
γ
t
µ
t
∆
t
(24)
γ
t
=
β
µ
t
∆
t
+
β
(25)The fusion process ensures the optimal fusion of
S E
q
ω,t
and
S E
q
∇
,t
assuming that the convergence rate of
S E
q
∇
governedby
α
is equal or greater than the physical rate of change of orientation. Therefore
α
has no upper bound. If
α
is assumedto be very large then
µ
t
, deﬁned by equation (22), alsobecomes very large and the equations simplify. A large valueof
µ
t
used in equation (20) means that
S E
ˆ
q
est,t
−
1
becomesnegligible and the equation can be re-written as equation (26).
S E
q
∇
,t
≈−
µ
t
∇
f
∇
f
(26)The deﬁnition of
γ
t
in equation (25) also simpliﬁes if the
β
term in the denominator becomes negligible and the equationcan be rewritten as equation (27). It is possible from equation(27) to also assume that
γ
t
≈
0
.
γ
t
≈
β
∆
tµ
t
(27)Substituting equations (4), (26) and (27) into equation (23)directly yields equation (28). It is important to note that inequation (28),
γ
t
has been substituted as both as equation (26)and
0
.
S E
q
est,t
=
β
∆
tµ
t
−
µ
t
∇
f
∇
f
+(1
−
0)
S E
ˆ
q
est,t
−
1
+
S E
˙
q
ω,t
∆
t
(28)Equation (28) can be simpliﬁed to equation (29) where
S E
˙
q
est,t
is the estimated orientation rate deﬁned by equation(30).
S E
q
est,t
=
S E
ˆ
q
est,t
−
1
+
S E
˙
q
est,t
∆
t
(29)
S E
˙
q
est,t
=
S E
˙
q
ω,t
−
β
∇
f
∇
f
(30)It can be seen from equations (29) and (30) that thealgorithm calculates the orientation
S E
q
est
by numerically inte-grating the estimated rate of change of orientation
S E
˙
q
est
. Thealgorithm computes
S E
˙
q
est
as the rate of change of orientationmeasured by the gyroscopes,
S E
˙
q
ω
, with the magnitude of the gyroscope measurement error,
β
, removed in a directionbased on accelerometer and magnetometer measurements.Fig.1 shows a block diagram representation of the completeorientation estimation algorithm implementation for an IMU.
Accelerometer
S
ˆ
a
t
Gyroscope
S
ω
tS E
ˆ
q
est,tS E
˙
q
est,t
12
S E
ˆ
q
est,t
−
1
⊗
S
ω
t
.dt
q
q
J
T g
(
S E
ˆ
q
est,t
−
1
)
f
g
(
S E
ˆ
q
est,t
−
1
,
S
ˆ
a
t
)
z
−
1
z
−
1
∇
f
∇
f
β
Fig. 1. Block diagram representation of the complete orientation estimationalgorithm for an IMU implementation
D. Magnetic distortion compensation
Investigations into the effect of magnetic distortions on anorientation sensor’s performance have shown that substantialerrors may be introduced by sources including electrical appli-ances, metal furniture and metal structures within a buildingsconstruction [30], [31]. Sources of interference ﬁxed in thesensor frame, termed hard iron biases, can be removed throughcalibration [32], [33], [34], [35]. Sources of interference in theearth frame, termed soft iron errors, may only be removedif an additional reference of orientation is available. Anaccelerometer provides a reference of attitude and so maybe used to compensate for inclination errors in the measuredearth’s magnetic ﬁeld.The measured direction of the earth’s magnetic ﬁeld in theearth frame at time
t
,
E
ˆ
h
t
, can be computed as equation(31). The effect of an erroneous inclination of the measureddirection earth’s magnetic ﬁeld,
E
ˆ
h
t
, can be corrected if thealgorithm’s reference direction of the earth’s magnetic ﬁeld,
E
ˆ
b
t
, is of the same inclination. This is achieved by computing
E
ˆ
b
t
as
E
ˆ
h
t
normalised to have only components in the earthframe
x
and
z
axes; as described by equation (32).
E
ˆ
h
t
=
0
h
x
h
y
h
z
=
S E
ˆ
q
est,t
−
1
⊗
S
ˆ
m
t
⊗
S E
ˆ
q
∗
est,t
−
1
(31)
E
ˆ
b
t
=
0
h
2
x
+
h
2
y
0
h
z
(32)Compensating for magnetic distortions in this way ensuresthat magnetic disturbances are limited to only affect theestimated heading component of orientation. The approachalso eliminates the need for the reference direction of theearth’s magnetic ﬁeld to be predeﬁned; a potential disad-vantage of other orientation estimation algorithm [12], [19].Fig.2 shows a block diagram representation of the completealgorithm implementation for a MARG sensor array, includingthe magnetic distortion compensation.
E. Algorithm adjustable parameter
The orientation estimation algorithm requires 1 adjustableparameter,
β
, representing the gyroscope measurement errorexpressed as the magnitude of a quaternion derivative. Itis convenient to deﬁne
β
using the angular quantity
˜
ω
max
182

Search

Similar documents

Tags

Related Search

History of Science and Medicine In Medieval aThe use of bricolage and other multi-method aIslamic State Of Iraq And The LevantOffice Of Management And BudgetInstitute Of Electrical And Electronics EnginUnited States Department Of Housing And UrbanSeparation Of Church And StateA Song Of Ice And FireEconomics Of Transport And Utility IndustriesSeparation Of Church And State In The United

We Need Your Support

Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks