An Adaptive Complementary Filter For Gyroscope/Vision Integrated Attitude Estimation

Transcription

1 Paper Int l J. of Aeronautcal & Space Sc. 17(), 1 1 (16) DOI: An Adaptve Complementary Flter For Gyroscope/Vson Integrated Atttude Estmaton Chan Gook Park* Department of Mechancal & Aerospace Engneerng/ Automaton and Systems Research Insttute, Seoul Natonal Unversty, Seoul 886, Republc of Korea Chang Ho Kang** Department of Mechancal & Aerospace Engneerng, Seoul Natonal Unversty, Seoul 886, Republc of Korea Sanghyun Hwang*** and Chul Joo Chung**** Agency for Defense Development, Daejeon 3186, Republc of Korea Abstract An atttude estmaton algorthm whch ntegrates gyroscope and vson measurements usng an adaptve complementary flter s proposed n ths paper. In order to make the flter more tolerant to vson measurement fault and more robust to system dynamcs, fuzzy nterpolator s appled. For recognzng the dynamc condton of the system and vson measurement fault, the cut-off frequency of the complementary flter s determned adaptvely by usng the fuzzy logc wth desgned membershp functons. The performance of the proposed algorthm s evaluated by experments and t s confrmed that proposed algorthm works well n the statc or dynamc condton. Key words: Adaptve complementary flter, Fuzzy logc, Gyro/Vson ntegrated atttude estmaton 1. Introducton Accordng to development of the recent electromechancal technque, study on atttude estmaton usng mcro electro-mechancal system (MEMS) based nertal sensors has been ncreased and ts result s appled for varous systems [1]. The nertal measurement unt (IMU) conssts of three axs gyroscopes and accelerometers. The MEMS gyroscope measures angular rate of the system and the accelerometer measures specfc forces of the system where IMU s mounted []. In conventonal nertal sensor based systems, the computaton of system s atttude s accomplshed by ntegratng the angular rate obtaned from the gyroscope [3]. However, because the gyroscope suffers from hgh drft and nose, atttude estmaton by usng gyroscope wll deterorate over tme. On the other hand atttude derved from accelerometer does not dverge wth tme n the absence of moton acceleraton. Thus, accelerometer can be used as a compensatng sensor for correctng the drft of the gyroscope. In general case, data fuson algorthms are mplemented to ntegrate two nformaton sources from the gyroscope and accelerometer, respectvely by usng extended Kalman flter [, 5] or complementary flter [6, 7, 8]. However, accelerometer cannot clearly dstngush nclnaton and acceleraton. When the system operates n dynamc status, atttude estmaton based on these methods wll be less accurate. In order to handle ths problem, vson data s used to compensate the drft error of gyroscope n ths paper. The vson data s obtaned from stereo camera and atttude estmaton error of vson based method does not dverge wth tme because t s not calculated va ntegraton but rely Ths s an Open Access artcle dstrbuted under the terms of the Creatve Commons Attrbuton Non-Commercal Lcense ( whch permts unrestrcted non-commercal use, dstrbuton, and reproducton n any medum, provded the orgnal work s properly cted. * Professor, Correspondng author: chanpark@snu.ac.kr ** Ph. D Student *** Senor researcher **** Researcher Receved: July 1, 15 Revsed: May, 16 Accepted: June 19, 16 Copyrght c The Korean Socety for Aeronautcal & Space Scences 1 pissn: 93-7x eissn: 93-8 (1~1) ndd 오후 8:7:5

2 Chan Gook Park An Adaptve Complementary Flter For Gyroscope/Vson Integrated Atttude Estmaton on the recognton of feature ponts n dynamc stuaton. Although ths method has long term atttude stablty, the atttude estmaton wth vson data has features trackng error n dynamc stuaton. For reducng the effect of vson data dsturbances on the atttude estmaton, adaptve logc s requred and one of algorthms s fuzzy logc [9, 1] whch has a characterstc to represent pror knowledge as fuzzy rules, whch can be used to dentfy the dynamc condton and the vson measurement fault. Thus, n ths paper, an adaptve complementary flter s proposed usng fuzzy logc whch s used to automatcally adjust the cut-off frequency accordng to dynamc states and the qualty of vson measurement. Performance of the proposed algorthm s verfed by comparng the conventon method wthout fuzzy logc n experments. The rest of the paper s organzed as follows. Secton ntroduces a bref descrpton of the atttude estmaton wth MEMS gyroscope and vson data. In secton 3, a complementary flter wth fuzzy logc s explaned. In addton, experment and smulaton results shown n secton and secton 5 presents bref conclusons fnally.. Atttude Estmaton In ths secton, atttude estmaton methods usng gyroscope and vson data obtaned by stereo-camera are ntroduced. Each method have unque characterstcs accordng to operatng envronment and the performance of atttude estmaton can be mproved by usng the ntegraton of two methods. In addton, t s mportant to select the approprate coordnate for determnng atttude of system. The navgaton frame whch refers to North-East-Down (NED) frame and the body frame are used as the and local frames, respectvely. The gyroscope measurement s algned wth the body frame consstng of orthogonal axes where x-axs s n the drecton of forward moton of the system, y-axs s n the transverse moton of the system, N bx Fg. 1. Reference frame, body frame and defnton of Euler angles b z D by E and z-axs n the down drecton as shown n Fg. 1. The navgaton frame and the body frame can be related by a sequence of rotatons about Euler angles. However, n ths paper, t s assumed that the ntal body frame s algned wth the ntal frame as shown n Fg. 1, whch s the result obtaned through algnment process of the gmballed system. Algnment s the process whereby the orentaton of the axes of a system s determned wth respect to the axs n strapdown system or the gmbal s physcally rotated untl two of the body axes are level wth respect to the axs n gmballed system. In many applcatons, t s essental to acheve an accurate algnment of a system wthn a perod of tme. However, n ths paper, we are focusng on atttude estmaton method nstead of algnment..1 Atttude Determnaton by Gyroscopes Atttude estmaton methods are classfed as quaternon method, drecton cosne matrx method, and Euler angle method. Among these, the Euler angle method whch updates the Euler angles (roll, ptch, and yaw) uses the relatonshp between the body angular rotaton rate and the dervatve of the Euler angles as shown n Fg. 1. Euler angles have the advantages of beng a more meanngful atttude expresson than ether the quaternon method or the drecton cosne matrx method, whch means that the user can recognze the atttude of the system ntutvely. The update process of the Euler rotatons of the body wth respect to the chosen frame s expressed as [11] 1 sntan costan x cos sn y sn / cos cos / cos z where ω represents the angular rate of the axs n the body frame. ϕ and ψ are three Euler angles, whch represent the roll, ptch and yaw, respectvely. The Euler angles can be obtaned by drectly ntegraton of (1), gven a known ntal condton.. Atttude Determnaton by Vson Data Atttude estmaton method usng vson data s based on the prevous work [1]. The atttude estmaton system shown n Fg. by vson data conssts of two nfrared CCD cameras (VCC-S7) and nfrared LEDs attached on the system, Matrox Meteor-MC/ frame grabber and a computer for the trackng algorthm. The vson data from two cameras s transmtted to the desktop computer and dgtzed to a resoluton of 6x8 pxels. Fg. 3 shows the flow chart of (1) 15 (1~1) ndd 오후 8:7:5

3 Int l J. of Aeronautcal & Space Sc. 17(), 1 1 (16) the mage processng algorthm [1]. Features (LED ponts) are extracted wth the threshold and wth the maskng technque of the mage plane. Then the -D pont sets s obtaned n the mage plane. By usng the eppolar lne, the Hausdorff dstance and the camera calbraton nformaton, the -D pont sets of two mages can be transformed to 3-D pont sets. After the model ndexng, 3-D pont sets ndexed wth the model can be obtaned. Fnally, the atttude and the poston of the system n body frame can be estmated by usng the pont sets. A more detaled mage processng technque s ntroduced n the prevous paper [1]. 3. Adaptve Complementary Kalman Flter The output of the prevous secton s two estmates of system s atttude from the gyroscope and the vson data, respectvely. As already mentoned, n order to mprove the estmaton performance, data fuson s performed by usng a complementary Kalman flter [6], whch operates only on the errors n prmary state varables and compensates for the dsadvantage of each estmaton result from gyroscope and vson data. Thus, ths method provdes relatvely accurate atttude estmaton compared to each output from the gyroscope and the vson data. Furthermore, n order to make the flter more robust to system dynamcs error and more tolerant to vson measurement fault, the complementary Kalman flter appled by fuzzy logc s proposed n ths paper. Fuzzy logc s desgned to recognze the system dynamcs status and the qualty of the vson measurement and adjust the fadng factor of the adaptve flter. 3.1 Conventonal Complementary Flter The hgh frequency response of the gyroscope s relable, whle ts low frequency response s unrelable because of the Fg.. Composton of the vson system [1] Fg. 3. The flow chart of the mage processng for atttude estmaton [1] DOI: 16 (1~1) ndd 오후 8:7:5

4 Chan Gook Park An Adaptve Complementary Flter For Gyroscope/Vson Integrated Atttude Estmaton drft. In case of the vson data, t s drft-free, but t may be contamnated by trackng falures of feature ponts n the hgh dynamc moton. Thus, the measurements of vson and gyroscope have contrary characterstcs that are sutable to combne each other by a complementary flter. The structure of the flter s shown n Fg.. Gyroscope measurements are transformed from angular rates of body frame to Euler rates by (1) and ϕ v s the atttude obtaned from vson data. The complementary flter compares atttude from the ntegraton of the Euler rate wth the atttude angle from the vson data and the error between them s fed-back through a proportonal and ntegral (PI) controller to compensate the gyroscope bas error, b [8]. The atttude result s expressed n Laplace form as follows KI g b g KP V V ˆ 1 1 ˆ ˆ s s s () where K P and K I are a proportonal gan and ntegral gan, respectvely. By rearrangng (), the atttude estmaton values can be expressed as s K s K ˆ g P I s KPsK I s s KPsKI As shown n (3), the frst transfer functon refers to hghpass flter and the second one s low-pass flter. Thus, atttude can be estmated by ntegratng the atttude derved from the gyroscope through the hgh-pass flter and the atttude obtaned from the accelerometers through a low-pass flter. The PI gans are related to the cut-off frequency of the flter and the dampng rato. Relatons among PI gans, cut-off frequency and dampng rato are as follows: KI K P v (3) () where ω s the cut-off frequency and ζ s dampng rato whch s fxed to.77 to provde a good transent response [8, 1]. The complementary flter usng fxed cut-off frequency s dffcult to satsfy acceptable performance n dynamc condton because atttude error caused by vson data dsturbance s added to the flter. Thus, the cut-off frequency should be adjusted to mprove the performance of the flter. 3. Fuzzy Logc for Adaptve Complementary Flter In order to adjust the cut-off frequency by usng a weghtng factor, fuzzy logc [13] s appled to the complementary flter as shown n Fg. 5. The nput varables of the fuzzy logc for dentfyng system dynamcs and vson measurement fault need to be defned. In the frst, gyroscope measurement s relatve to system movements, whch can be used as an dentfcaton parameter of system dynamcs. In terms of fault tolerance, normalzed square error obtaned by nnovaton nformaton at present epoch can be used as detecton parameter of vson measurement fault. Thus, the normalzed square error and the absolute value of the gyroscope measurement are exploted as nputs of the fuzzy logc and they are expressed as D 1 en n (5) Cn D n n n n (6) x y z where D 1 (n) s normalzed square error, C(n) s error varance whch refers to C(n)=E[e(n) ], and e(n) means one component of e n ˆ. V n n Ths error value, e(n) s a Gaussan random varable wth zero mean and varance C(n). As e(n) s normalzed by ts covarance, D 1 (n) s a ch-square KI K P g b ˆ V Fg.. The block dagram of the complementary flter 17 (1~1) ndd 오후 8:7:55

5 Int l J. of Aeronautcal & Space Sc. 17(), 1 1 (16) dstrbuted random varable wth n degrees of freedom,.e., D1 n n. In order to montor the qualty of the vson measurement, ch-square test can be mplemented wth fuzzy logc. D (n) denotes the moton grad n rotaton whch s used as crteron of system dynamcs. In order to desgn the membershp functons of the fuzzy sets, both nputs of the fuzzy logc are classfed as three grades, respectvely and functons are shown n Fg. 6 and 7. As shown n these fgures, L refers to Low, M represents medum, and H means hgh. In addton, F represents fault. The output of the fuzzy logc conssts of four sngleton membershp functons as follows: n 1 3 f n S f n VS f n ES f n F where <α 3 <α <α 1 <1, S represents small, VS s very small, ES s extra small, and F refers to fault status. By usng the output of the fuzzy logc, the cut-off frequency of the complementary flter s adjusted as (8) and the output of the fuzzy logc s called weghtng factor and s decded by (9). (8) c n n If D1 n s A and D n s (7) B, then (9) n n KI K P g b ˆ e V Fg. 5. The block dagram of the complementary flter wth Fuzzy logc 1 L M F D1 n Fg. 6. Membershp functon of the normalzed square error 1 L M H..5.6 D n Fg. 7. Membershp functon of the moton grade DOI: 18 (1~1) ndd 오후 8:7:55

6 Chan Gook Park An Adaptve Complementary Flter For Gyroscope/Vson Integrated Atttude Estmaton In (8), ω s nomnal cut-off frequency. Equaton (9) represents fuzzy rules of Takag-Sugeno fuzzy logc whch s commonly used and rather straghtforward to understand. A and B are fuzzy sets n the antecedent, α s the output value of the fuzzy system correspondng to the -th fuzzy rules wrtten n table 1. In addton, n refers to the fnal value of the fuzzy logc and s expressed by the weghed average of the as follows: 9 n n (1) 1 where s a normalzed weghtng factor whch s expressed as 9 j1 k 1 k j k 1 k (11) where k s the output of the membershp functon n the case of k-th nput and -th fuzzy rule wrtten n Table 1. Fnally, weghng factor should be selected accordng to dynamc status and vson measurement fault. If the system s experencng severe dynamc moton, the atttude estmated from vson data has huge error due to trackng falure of feature ponts. In ths case, smaller weghtng factor should be used. A small weghtng factor refers cut-off frequency s reduced and more relablty s gven to the gyroscope. Otherwse, f the system s n low dynamc condton, then more weghtng s put to the vson method. In addton, f normalzed square error exceeds 1.3 (3 percent ncrease of the normalzed square error compared to that of the expected value) regardless of dynamc condton, there s fault n vson data. Thus, n ths case, zero weghtng factor s selected as shown n table 1. Fg. 8. The helmet model s on the rate table and the stereo camera system s placed besde the rate table. In order to estmate atttude of the helmet model, unque patterns of LEDs are mplemented. In addton, a trple-axs rate table (Acutronc) s used as system and a trple-axs MEMS gyroscope (STIM 1) s used. Its bas nstablty s.5 o /h, angular random walk s.15 o / h, and temperature gradents s 1 o /h. The performance evaluaton s carred out by comparng wth conventonal complementary flter wthout the fuzzy logc. In addton, analyss on fault tolereance of flter s also performed n the smulaton. Fgure 9 shows the atttude estmaton result whch s represented by the Euler angles. In ths fgure, the black lne s the, the green lne refers to the result of the conventonal algorthm whch has not fuzzy logc, the red lne s the result of the proposed algorthm, and the blue lne s the result when the only vson measurement s used. RMSE of the proposed algorthm s.75, RMSE of the conventonal algorthm s.565 and that of vson data s.88. As shown n the enlarged plot of Fg. 9 n Fg. 1, 11, and 1, the proposed algorthm shows the. Experments Fg. 8. Expermental setup Ths secton descrbes smulaton results and the off-lne test wth the real data. The expermental setup s shown n Table 1. Fuzzy rules Table 1. Fuzzy rules Rule D1 n D n n 1 L L 1 L M S 3 L H VS M L S 5 M M VS 6 M H ES 7 F L 8 F M 9 F H Adaptve complementary(proposed) Complementary Vson only tme ndex tme ndex tme ndex Fg. 9. Atttude estmaton results (Top: roll angle, Mddle: ptch angle. Bottom: yaw angle) 19 (1~1) ndd 오후 8:7:56

7 Int l J. of Aeronautcal & Space Sc. 17(), 1 1 (16) more accurate result than other methods. However, n some cases of dynamc moton (especally the atttude change at tme ndex 33 n Fg.11), there s the performance reducton due to the response of the complementary flter whch has senstve response n dynamc stuaton. Mostly, ts characterstc helps to mprove the atttude estmate n a sudden change of atttude, but the characterstc occasonally causes an overshoot n normal condton as shown n Fg. 11. In order to reduce ths effect on the estmaton performance, the proportonal gan and ntegral gan of the complementary flter should be reset accordng to the system response. In Fg. 13, vson data faults caused by trackng falure of feature ponts are added at 18 tme ndex and 8 tme ndex. In addton, ts effect lasts for tme ndex. The fault s modeled by a step-type fault whch reflects characterstc of the vson data and ts value s set to 5. Smulaton results demonstrate the proposed algorthm performs well even under vson fault case because the proposed algorthm adjust the cut-off frequency accordng to the stuaton by usng fuzzy logc. RMSE of the proposed algorthm s.6, RMSE of the conventonal algorthm s.6938 and that of vson 1 data s In Fg. 1, t s confrmed that Adaptve complementary(proposed) Complementary Vson only Adaptve complementary(proposed) Complementary Vson only tme ndex Fg. 1 Atttude estmaton results 1 (enlarged roll angle) tme ndex Fg. 1. Atttude estmaton results 1 (enlarged roll angle) - the cut-off frequency of the proposed algorthm (red dot lne) s adjusted by the fuzzy rules n order to reduce the effect of the bas type fault n the vson data. Whereas the cut-off frequency of the adaptve complementary flter whch have no fault detecton rules n the fuzzy rules [8] does not change when the bas type fault occurs. In the case of dynamc stuaton (7~3 tme ndex), both of the adaptve complementary flter adjust the cut-off frequency through the fuzzy rules (rule 1~6) n table tme ndex tme ndex Fg. 1 Atttude estmaton results 3 (enlarged yaw angle) Fg. 1. Atttude estmaton results 3 (enlarged yaw angle) Fg. 1 Atttude estmaton results 3 (enlarged yaw angle) 1 Adaptve complementary (proposed) 1 Adaptve complementary (W/O the fuzzy rules 7~9) 1 Complementary Vson Adaptve only 8 complementary (proposed) 1 Adaptve complementary (W/O the fuzzy rules 7~9) Complementary 6 Vson only tme ndex Fg. 13 Atttude estmaton results wth vson data fault Fg. 13. Atttude estmaton results wth vson data fault tme ndex Fg. 13 Atttude estmaton results wth vson data fault cut-off freq.(dgtal) tme ndex Adaptve complementary (proposed) Adaptve complementary (W/O the fuzzy rules 7~9) tme ndex Fg. 11. Fg. Atttude 11 Atttude estmaton estmaton results results (enlarged (enlarged ptch ptch angle) angle) Fg. 1. Adjustment Fg. 1 Adjustment result of the result cut-off the frequency cut-off frequency DOI: (1~1) ndd 16-3 오후 8:7:57

8 Chan Gook Park An Adaptve Complementary Flter For Gyroscope/Vson Integrated Atttude Estmaton 5. Conclusons An adaptve complementary flter usng fuzzy logc has been proposed to estmate the atttude of the system usng MEMS gyroscope and vson data obtaned by stereo camera. The fuzzy logc s used to recognze dynamc status of the system and vson data fault by adjustng the cut-off frequency. In ths way, t can make the flter more tolerant regardng vson data fault. The performance of the proposed algorthm s verfed by smulatons and ts results show that the proposed atttude estmaton method has better performance compared wth conventonal method. Acknowledgement Ths work has been supported by the Dual Use Technology Program, 11-DU-EE-1 and the Mnstry of Scence, ICT & Future Plannng of Republc of Korea under Space Core Technology Development Program (Project number NRF- 13M1A3A3A68) References [1] Bachmann, E. R., Duman, I., Usta, U. Y., Mcghee, R. B., Yun, X. P. and Zyda, M. J., Orentaton Trackng for Humans and Robots Usng Inertal Sensors, Computatonal Intellgence n Robotcs and Automaton, Internatonal symposum of the IEEE, 1999, pp [] Geger, W., Bartholomeyczk, J., Breng, U., Gutmann, W., Hafen, M., Handrch, E. and Zmmermann, S., Mems IMU for AHRS Applcatons, Poston, Locaton and Navgaton Symposum, Internatonal symposum of the IEEE/ION, 8, pp [3] Jung, D. and Tsotras, P., Inertal Atttude and Poston Reference System Development for a Small UAV, AIAA Infotech at aerospace, 7, pp [] Brown, R. and Hwang, P., Introducton to Random Sgnals and Appled Kalman Flterng, Wlley & Sons, 199. [5] Kang, C. W., Yoo, Y. M. and Park, C. G., Performance Improvement of Atttude Estmaton Usng Modfed Euler Angle Based Kalman Flter, Journal of Insttute of Control, Robotcs and Systems, Vol. 1, Issue 9, 8, pp [6] Foxln, E., Inertal Head-tracker Sensor Fuson by a Complementary Separate-bas Kalman Flter, Vrtual Realty Annual Internatonal Symposum, Proceedngs of the IEEE, 1996, pp [7] Mahony, R., Hamel, T. and Pflmln, J. M., Nonlnear Complementary Flters on the Specal Orthogonal Group, IEEE Transactons on Automatc Control, Vol. 53, Issue 5, 8, pp [8] Shen, X., Yao, M., Ja, W. and Yuan, D., Adaptve Complementary Flter Usng Fuzzy Logc and Smultaneous Perturbaton Stochastc Approxmaton Algorthm, Measurement, Vol. 5, Issue 5, 1, pp [9] Loebs, D., Sutton, R., Chudley, J. and Naeem, W., Adaptve Tunng of a Kalman Flter va Fuzzy Logc for an Intellgent AUV Navgaton System, Control engneerng practce, Vol. 1, Issue 1,, pp [1] Kang, C. W. and Park, C. G., Atttude Estmaton wth Accelerometers and Gyros Usng Fuzzy Tuned Kalman Flter, European Control Conference, Proceedngs of the EUCA, 9, pp. 3. [11] Ttterton, D. and Weston, J. L., Strapdown Inertal Navgaton Technology, IET, UK,. [1] Heo, S., Shn, O. and Park, C. G., Estmatng Moton Parameters of Head by Usng Hybrd Extended Kalman Flter, ION GNSS 9, Proceedngs of ION, 9, pp [13] Takag, T. and Sugeno, M., Fuzzy Identfcaton of Systems and ts Applcatons to Modelng and Control, IEEE Transactons on Systems, Man and Cybernetcs, Vol. 15, Issue 1, 1985, pp [1] Metn, N., Pflmln, J. M., Hamel, T. and Soueres, P., Atttude and Gyro Bas Estmaton for a Flyng UAV, Intellgent Robots and Systems, Internatonal Conference on IEEE/RSJ, 5, pp (1~1) ndd 오후 8:7:57