School Work

Madgwick_Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm_ICORR2011

IMU sensors, Accel Gyro
of 7
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
  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 efficient, 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 significantly 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 fields 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: 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: Harrison is with the Department of Mechanical Engineering, Uni-versity of Bristol, e-mail: of significant utility in this realm. In a recent survey, Zhoua[7], cited real time operation, wireless properties, correctnessof data, and portability as major deficiencies 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 specificenvironment 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 sufficient 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 field. 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 filter [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 reflected 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 filter 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 filters [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 filter 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 filters and verified 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-fies the experimental testing and accuracy of the algorithm andcompares it to existing systems. Section VII expands brieflygives 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  ω  defined 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 field  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 field. If the direction of an earth’s field is knownin the earth frame, a measurement of the field’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 infinite solutionsrepresented by all those orientations achieved by the rotationthe true orientation around an axis parallel with the field.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 predefined reference directionof the field in the earth frame,  E   ˆ d , with the measured fieldin the sensor frame,  S  ˆ s ; thus solving (5) where equation (6)defines 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 defined 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 field predefined in any direction.However, if the reference direction of the field is defined 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 defines 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 simplified objectivefunction and Jacobian defined 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 field can be considered to have com-ponents in one horizontal axis and the vertical axis; the verticalcomponent due to the inclination of the field 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 simplified objective function and Jacobian definedby 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 field alone will not provide a uniqueorientation of the sensor. To do so, the measurements and refer-ence directions of both fields 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 defined by a line, the solution surface defineby equation (18) has a minimum define 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   defined 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 filter 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 define  γ  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 , defined 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 definition of   γ  t  in equation (25) also simplifies 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 simplified to equation (29) where S E   ˙ q est,t  is the estimated orientation rate defined 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 fixed 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 field.The measured direction of the earth’s magnetic field 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 field,  E  ˆ h t , can be corrected if thealgorithm’s reference direction of the earth’s magnetic field, 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 field to be predefined; 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 define  β   using the angular quantity  ˜ ω max 182
Similar documents
View more...
Related Search
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