A Quaternion-Based Orientation Estimation Algorithm with Adaptive Interpolation for Wearable Computing

Transcription

1 23 8th International Conference on Communications and Networking in China (CHINACOM) A Quaternion-Based Orientation Estimation Algorithm with Adaptive Interpolation for Wearable Computing Xuan-cheng Zhou, Jian-in Chen, Xi-ruo Lu and Yi Dong Ke Lab of Broadband Wireless Communication and Sensor Network Technolog (Nanjing Universit of Posts and Telecommunications), Ministr of Education Nanjing, China, 23 Abstract The motion capture sstems that have rigorous real time requirements are often applied in wearable computing, aerospace and other fields. This paper presents a quaternionbased Orientation Estimation Algorithm (OEA) with low compleit using a Micro-Electro-Mechanic Sstems (MEMS) triais accelerometer, a tri-ais groscope and a tri-ais magnetometer. We give the description of the orientation of rigid bod and propose an adaptive interpolation algorithm for data fusion. Eperimental results demonstrate that the proposed OEA ma track the orientation accuratel and consume lower power as well as processing resources in comparison to the etended kalman filtering algorithm. Inde Terms Motion capture, real time, orientation estimation, quaternion. I. INTRODUCTION With the development of Wireless Sensor Network (WSN), its branch (e.g., Bod Sensor Network (BSN)), has currentl drawn an etensive attention from a growing number of researchers. A host of intelligent inertial sensors can be integrated into BSN to realize various functions to facilitate human s dail life in fields ranging from healthcare domain to sports domain []. A set of inertial sensors on a patient can monitor abnormalities of the patient. For eample, he/she has a heart attack or falls down accidentall. Likewise, a suit of these sensors on a trampoline plaer bod can capture the motion of his limbs convenientl, and accuratel find the movement that needs to be improved. These motion capture sstems that have rigorous real-time requirements are often applied in aerospace, robotics and other fields [2]. Using inertial sensors to track motion is so appealing owing to the fact that the inertial sensors are not confined to the dependence of eternal auiliar devices in comparison to the camera-based motion tracking sstem. A self-contained integrated sensor module will satisf the motion capture requirements [3]. Some researchers carr out the motion tracking technique onl using accelerometers in a relativel simple wa [4] while some others are using a sophisticated method with accelerometers, groscopes and magnetometers combinedl [5],[6],[7]. In spite of the widespread of the Inertial Measurement Units (IMUs), there still eist some problems. IMUs onl maintain high precision in short term and suffer from severe drift errors as well as accumulated errors in long term. Accordingl, etensive research focuses on how to reduce the errors generated from IMUs. For eample, X.P. Yun et al. [8],[9] proposed factored quaternion algorithm (FQA) to track human motion and a quaternion-based adaptive-gain complementar filter using Laplace transform. He Zhao and Zheao Wang [] used ultrasonic sensors with inertial sensors to realize motion tracking with etended kalman filter. However, these algorithms are not appropriate for wearable computing. A.D. Young [] presented a complementar filter for data fusion with low computational compleit. In this paper, we propose a quaternion-based Orientation Estimation Algorithm (OEA) with lower compleit than A.D. Young s, especiall designed for real-time wearable computing. The rest of paper is organized as follows. Section Ⅱ introduces the fundamentals of IMU detailedl while section Ⅲ describes the proposed data fusion algorithm with low compleit. Then, section Ⅳ reports the eperimental results of OEA. The last section draws the conclusions and gives our future work. II. OVERVIEW OF IMU This section introduces different methods to track the orientation of an object with inertial sensors. The IMU usuall consists of several inertial sensors, such as a 3-ais accelerometer, a 3-ais groscope and a 3-ais magnetometer. We use these sensors together to achieve the purpose of tracking orientation. A. Groscope In general, just the use of a 3-ais groscope can estimate the orientation., and are the angular rate measurements from a 3-ais groscope and q is the quaternion of representation of quaternion. Then the quaternion rate will be calculated using the current angular rates, and and the previous orientation quaternion : q ˆ q, () 2 where ω is a pure quaternion and the quaternion multiplication is applied in this equation. To be more specific, () can be transformed into: IEEE

2 Figure. A two-dimensional scenario to show pitch angle w ˆ ˆ ˆ z z ˆ w ˆ ˆ z, (2) z q 2 ˆ w ˆ z ˆ z z ˆ w ˆ ˆ z where,, and are the four components of the previous orientation quaternion. On this scenario, the current orientation denoted with quaternion is: ˆ w w ˆ q ˆ q q T T, (3) d s s ˆ ˆ z z where is sensor sampling period. B. Accelerometer and Magnetometer We use a 3-ais accelerometer and a 3-ais magnetometer to track the orientation of an object moving slowl. Here,, and are the linear accelerations from a 3-ais accelerometer,, and are the magnetic measurements from a 3-ais magnetometer. θ, ψ and φ stand for rotation angles about the -ais, -ais and z-ais, i.e., pitch angle, aw angle and roll angle, respectivel. As we know, an rotation in 3D can be viewed as a combination of three separate rotations about each ais in the global frame with a specific sequence. Such sequence of combining the three rotations is adopted in this paper: rotating about -ais b θ, rotating about z-ais b φ and rotating about -ais b ψ. ) Pitch angle It is known that when the bod moves slowl or is stationar, the accelerometer output approimatel equals the value of gravit. In order to understand the theor of calculating pitch angle using accelerometer, it is useful to take a two-dimensional scenario as an eample. As Fig. shows, the object is static and the 2-ais accelerometer attached to the object can measure the acceleration in ais and in ais. Obviousl, angle here is the pitch angle. And the relation between the local gravit g and the measurements are: a g cos. (4) a g sin Figure 2. A depict of a three-dimensional scenario to estimate pitch angle Therefore, the pitch angle here can be epressed as: a arctan. (5) a Fig. 2 depicts a three-dimensional scenario to estimate the pitch angle θ. The coordinate sstem is a local coordinate sstem while the coordinate sstem is a global coordinate sstem., and are the accelerations from a 3-ais accelerometer. The pitch angle θ is defined as the angle between ais and plane. Accordingl, after we etend the two-dimensional scenario mentioned above to threedimensional scenario, the pitch angle θ can be determined as: a arctan. (6) 2 2 a a Then, quaternion of rotation about -ais b θ angle can be computed using the definition of a quaternion as follows: w cos 2. (7) q sin 2 z 2) Roll angle So as to figure out roll angle in the net rotation about z- ais, it is necessar to see Fig. 2 from another side. As Fig. 3 shows, roll angle φ is defined as the angle between -ais and intersection of the plane with plane. In terms of using the similar theor of (6), angle α and angle β in Fig. 3 can be epressed as: a arctan 2 2 a a. (8) z az arctan 2 2 a a In this case, the roll angle φ is: sin arctan. (9) sin Furthermore, (9) is simplified as: z 847

3 3-ais accelerometers 3-ais groscopes 3-ais magnetometers } Block Selection Dnamic Block Static Block Data Fusion Algorithm Figure 3. Roll angle estimation Figure 4. Yaw angle estimation a arctan. () a It is noted that when -ais of local coordinate sstem is close to -ais of global coordinate sstem, the value of φ will change dramaticall in a relativel short period. It means the outputs from an accelerometer in -ais and -ais are close to zero at the same time. On such conditions, the roll angle should be set to zero or a small value close to zero. Thus, the quaternion representing the rotation in this step can be estimated b: w cos 2. () q z sin 2 3) Yaw angle The 3-ais magnetometer can measure the local magnetic field intensit of the local coordinate sstem. So the usage of one 3-ais magnetometer can ield the aw angle of rotation about -ais with the following equation: m arctan D, (2) m z Figure 5. Block diagram of orientation estimation algorithm where, and are the three outputs from a 3-ais magnetometer in a local coordinate sstem and D is the declination angle between magnetic North and geographic North. However, if there is onl one magnetometer to calculate the aw angle ψ, the computed result will deviate from the true aw angle to a large etent in man scenarios. Hence, an acceleration-based compensation algorithm is introduced here. Fig. 4 illustrates a simplified model of the algorithm. In Fig. 4, the coordinate sstem is a local coordinate sstem. It is necessar to define a plane coordinate sstem. -ais is the projection of -ais onto plane while -ais is perpendicular to -ais. Project the measured magnetic readings, and onto plane and decompose the three vectors along -ais and -ais on plane. We assume, are the decomposed components along -ais and -ais on plane, respectivel. Finall, and can be computed as: h m cos m cos z h m cos cos m cos m cos cos z 4 (3) Afterwards, aw angle ψ can be obtained using (2) after, in (2) are substituted for, in (3). The quaternion of aw angle can be epressed as: w cos 2. (4) q sin z 2 Accordingl, the complete static orientation epressed b quaternion is then given b: q q q q, (5) s Estimated Quaternion-based Orientation 848

4 q (-t)δ qt 4 2 tδ Figure 6. A simplified schematic of quaternion interpolation where the product between quaternions is the quaternion multiplication. q Interpolation times III. ORIENTATION ESTIMATION ALGORITHM In this section, we propose a quaternion-based Orientation Estimation Algorithm (OEA). Fig. 5 shows the block diagram of OEA. The OEA includes two main blocks. One is a dnamic block using a 3-ais groscope and the other is a static block with a 3-ais accelerometer and a 3-ais magnetometer. To achieve better performance, a data fusion algorithm is proposed for OEA. When an object is moving towards an direction rapidl or is subject to ecessive vibration, its acceleration ma no longer represent the gravit in the specified field. On this scenario, it is believed that the outputs from the groscope are more reliable than the data from the accelerometer. However, if the object moves slowl or is stationar, it is more convincing that the accelerometer offers more accurate information about orientation estimation since integration errors as well as drift errors of the groscope are inevitable. Furthermore, in this case, the previous integration errors caused b the groscope will also be eliminated since orientation estimation using the accelerometer and the magnetometer is independent on the previous orientation status. A. Block Selection As described before, if the bod is still or moves slowl, the static block is selected. On the contrar, the dnamic block works in case that the bod moves rapidl. In the best condition, the total angular velocit should be relativel low (lower than a threshold of ) and the measurements from the accelerometer mainl represent local gravit when the bod moves slowl, which indicates the difference between the acceleration readings and local gravit value is lower than a threshold of. Consequentl, static block is performed if the measurements meet the following conditions at the same time: a ALL ALL g g, (6) where g, and are the gravit value, the sum of angular velocit and the sum of acceleration in the global coordinate sstem, respectivel. Then, if the measurements do not satisf (6), the dnamic block will work. B. Data Fusion Algorithm with Low Computational Compleit If the accumulated errors and drift errors are relativel high due to the usage of previous angular velocit in dnamic block for quite a while, the orientation computed of the object ma be Pitch angle(degrees) Roll angle(degrees) Yaw angle(degrees) Coefficient t Figure 7. Relation between interpolation coefficient and interpolation times Sampling number Figure 8. Orientation estimated b static block with three 9º rotations different from the true orientation. Hence, when the static orientation is computed in the above static block, an abrupt jump of orientation ma appear. In order to get a smooth transformation from the former movement to the current movement, a spherical linear interpolation algorithm of quaternion with adaptive interpolation coefficient is considered here. Fig. 6 illustrates the model of interpolation algorithm., and are assumed to be three quaternions and is an interpolated quaternion from to. Besides, δ is the angle between and, while t is the interpolation coefficient ranging from to. Using sine law, is then determined b: sin( t) sin t q q q. (7) t sin sin It is interesting to note that when t is equal to, will be and when t is equal to, will be. After iterative calculating (7) repeatedl, which denotes that is replaced b at the net time, smooth transformation of quaternion from to can be completed b adjusting the interpolation coefficient t optimall. In addition, there are two cases to be noted here. First, if the angle δ is small, the denominator sin will be close to zero. In order to circumvent the potential mistake, another equation is emploed: q q t q q. (8) t 849

5 w z w Sampling number Interpolation Complementar - - EKF Figure 9. Comparison between three processing algorithms in slow movement Second, since the quaternion and quaternion represent the same orientation on a basis of fundamental properties of quaternion, the interpolated quaternion ma be quite different from the epected interpolated quaternion when and are suffering from this situation of ambiguit. To combat this problem, we alter the signs of and so that the dot product between and is nonnegative. To demonstrate how the coefficient t influences the performance of interpolation, Fig. 7 depicts relations between interpolation coefficient t and computing times while comes close to. It is noted that as the coefficient t is getting larger, the interpolation times are getting less. In normal 3D engine, displaing 3 frames per second is a satisfactor performance. For the purpose of ielding comfortable performance, the coefficient t is set according to the angle between the two quaternions and. In other words, the larger of angle is, the more interpolation times are needed to displa a smooth transformation between the two quaternions. The included angle δ between the two quaternions is given b: cos q q,8 2, (9) where the product is a dot product and cos is confined to (,]. After calculating the included angle δ, we set the coefficient t to cos. For instance, if δ is equal to 6, cos will be equal to.6. Then, the coefficient t will be set to.6. It can be seen from the Fig. 7 that when t is equal to.6, the interpolation will be implemented 7 times, which means that rotating the former quaternion displaed on the screen to the epected quaternion of the true orientation needs about 7/3(s) in the case that the static block is producing the same quaternion. It is noteworth that the relation between the angle δ and interpolation coefficient t satisfing the described purpose is not unique, this kind of relation is emploed empiricall for better performance. IV. EXPERIMENTAL ANALYSIS Our IMU is comprised of a Freescale s MMA736 tri-ais accelerometer (with a SingalQuest s SQ-SEN2 tilt/vibration Time consumed (s) z Sampling number Interpolation Complementar - - EKF Figure. Comparison between three processing algorithms in fast movement Complementar algorithm Interpolation algorithm st 2nd 3rd 4th 5th Average Times Figure. Time consumed b two algorithms sensor) [2], an InvenSense s 5 series MEMs tri-ais groscope (with ± 5 º /s full scale range and 2mv/ º /s sensitivit) [3], a Honewell s HMC5843 tri-ais magnetometer (with ±4.5Ga measurement range and 7mGa resolution) [3], 8MHz MSP43 CPU and an radio chip (CC242) [3]. Such a complete unit is approimatel 5g. Another device with an radio chip (CC242) is responsible for forwarding the data received from the integrated sensor unit to laptop through RS-232 interface. Post-processing will be completed in OEA. In the following eperiments, the sampling frequenc of the sensor was set to 5Hz and the threshold of angular velocit utilized in the selection block was chosen as γω= rad/s 57.3 deg/s and the threshold of acceleration was γg= m/s 2 so that the selection block could generate epected performance during practical eperiments. A. Orientation Estimation of the Static Block in OEA To validate the OEA presented in this paper, three separate rotations about three aes were performed. Fig. 8 shows the performance of the estimation algorithm. The IMU was rotated about -ais, z-ais and -ais b approimatel 9 degrees slowl in sequence and it was reset to the initial orientation when each rotation about the certain ais was completed. The slight vibration produced in the eperiment was caused b the tester 85

6 manuall. It can be found that the static block in OEA could track the orientation accuratel. In net eperiment, to show the performance of the data fusion block, the complementar filtering algorithm proposed b A. D. Young and the Etended Kalman Filter (EKF) designed for QUaternion ESTimator (QUEST) algorithm proposed b X. Yun were compared with the adaptive interpolation algorithm. The rotations in the above were operated again in this eperiment using adaptive interpolation algorithm in data fusion block and the two other algorithms mentioned above. Fig. 9 illustrates the comparison among the orientation estimated b three algorithms. It can be seen from the plot that three lines mainl overlap with each other. To be more specific, the results from the interpolation algorithm match better with the results from EKF algorithm than those from complementar filtering algorithm. To show the performance of our algorithm further, the eperiment of normal walking was then performed. An IMU was mounted on the instep of the foot to track the data from inertial sensors while the wearer keeps walking. As Fig. shows, the results from three algorithms are similar and share the same trend of fluctuation. The dotted line (interpolation algorithm) is surrounded b the solid line (complementar algorithm) and dashed line (EKF). The interpolation algorithm can track the orientation relativel accuratel. B. Computational Compleit of the Data Fusion Algorithm In [], A. D. Young compared his complementar filtering algorithm with EKF algorithm in time compleit and drew a conclusion that the complementar algorithm processed 7-9 times faster than the EKF algorithm with samples. The time compleit eperiment was then implemented to make a comparison between the interpolation algorithm and complementar algorithm. In order to reduce the errors produced b processor and operation manuall, 3 samples were processed times b each algorithm and the average time was taken as the processing time of the algorithm. Fig. shows the 5 times of the all eperiments and it can be seen from the plot that the interpolation algorithm is faster than the complementar algorithm b approimatel 7%. V. CONCLUSIONS This paper presents an orientation estimation algorithm with an adaptive interpolation algorithm for data fusion. Owing to its lower computational compleit, the proposed algorithm is more suitable for wearable computing and is capable to satisf the real-time processing requirements in most scenarios. This is due to the fact that our algorithm requires less computational compleit and produces less processing latenc than EKF. In our future work, since full bod tracking of a human requires at least 9 nodes, it cannot be guaranteed that ever node eperiences the same processing and transmission dela without the help of advanced techniques [4]. A more sophisticated network is considered for multi-node communications on the account of the fact that the networks topologies can both provide multi-hop and path diversit [5],[6] to support full-bod tracking in real time [7]. This work has been supported b the State Ke Development Program of Basic Research of China (23CB3295), National Natural Science Foundation of China (Grant No. 6265), the open research fund of Ke Lab of Broadband Wireless Communication and Sensor Network Technolog (Nanjing Universit of Posts and Telecommunications), and Ministr of Education (No. NYKL236), and Nanjing Universit of Posts and Telecommunications Foundation (Grant No. NY232). REFERENCES [] Z. Zhang, J. Pansiot, B. Lo, and G. Yang, "Human Back Movement Analsis Using BSN," in Bod Sensor Networks (BSN), International Conference on, pp. 3-8, 2. [2] J. Chen, L. Zhou, Y. Zhang, and D. F. Ferreiro, "Human Motion Tracking With Wireless Wearable Sensor Network: Eperience and Lessons," WBANs for Pervasive Healthcare, Consumer Electronics, and Entertainment Applications: Issues & Challenges, vol. 7, pp , 23. [3] R. Zhu and Z. Zhou, "A real-time articulated human motion tracking using tri-ais inertial/magnetic sensors package," Neural Sstems and Rehabilitation Engineering, IEEE Transactions on, vol. 2 (2), pp , 24. [4] C. Park, J. Liu, and P. H. Chou, "Eco: an ultra-compact low-power wireless sensor node for real-time motion monitoring," in Information Processing in Sensor Networks, Fourth International Smposium on, pp , 25. [5] V. Van Acht, E. Bongers, N. Lambert, and R. Verberne, "Miniature wireless inertial sensor for measuring human motions," in Engineering in Medicine and Biolog Societ, 29th Annual International Conference of the IEEE, pp , 27. [6] X. Yun, M. Lizarraga, E. R. Bachmann, and R. B. McGhee, "An improved quaternion-based Kalman filter for real-time tracking of rigid bod orientation," in Intelligent Robots and Sstems, 23 IEEE/RSJ International Conference on, pp , 23. [7] X. Yun and E. R. Bachmann, "Design, implementation, and eperimental results of a quaternion-based Kalman filter for human bod motion tracking," Robotics, IEEE Transactions on, vol. 22 (6), pp , 26. [8] X. Yun, E. R. Bachmann, and R. B. McGhee, "A simplified quaternionbased algorithm for orientation estimation from earth gravit and magnetic field measurements," Instrumentation and Measurement, IEEE Transactions on, vol. 57 (3), pp , 28. [9] J. Calusdian, X. Yun, and E. Bachmann, "Adaptive-gain complementar filter of inertial and magnetic data for orientation estimation," in Robotics and Automation (ICRA), 2 IEEE International Conference on, pp , 2. [] H. Zhao and Z. Wang, "Motion Measurement Using Inertial Sensors, Ultrasonic Sensors, and Magnetometers With Etended Kalman Filter for Data Fusion," Sensors Journal, IEEE, vol. 2 (5), pp , 22. [] A. Young, "Comparison of orientation filter algorithms for realtime wireless inertial posture tracking," in Wearable and Implantable Bod Sensor Networks, Sith International Workshop on, pp , 29. [2] Shimmer-Platform Datasheet. Available: [3] 9DoF-Spec-Datasheet. Available: [4] L. Zhou, H. Wang, and M. Guizani, "How mobilit impacts video streaming over multi-hop wireless networks?," Communications, IEEE Transactions on, vol. 6 (7), pp , 22. [5] L. Zhou, X. Wang, W. Tu, G.-M. Muntean, and B. Geller, "Distributed scheduling scheme for video streaming over multi-channel multi-radio multi-hop wireless networks," Selected Areas in Communications, IEEE Journal on, vol. 28, pp , 2. [6] L. M. Oliveira and J. J. Rodrigues, "Wireless sensor networks: a surve on environmental monitoring," Journal of communications, vol. 6, pp. 43-5, 2. [7] L. Zhou, R. Q. Hu, Y. Qian, and H.-H. Chen, "Energ-Spectrum Efficienc Tradeoff for Video Streaming over Mobile Ad Hoc Networks," Selected Areas in Communications, IEEE Journal on, vol. 3, pp , 23. ACKNOWLEDGEMENT 85