Research on Attitude Estimation Algorithm under Dynamic Acceleration

Abstract

In this paper, we propose the attitude estimation algorithm under the dynamic acceleration environment. Generally , an attitude sensor has biaxial or triaxial accelerometer in order to measure the direction of gravity. In this configration, the attitude sensor has serious error under the dynamic acceleration environment, because of the measurement error of gravity that is caused by the dynamic acceleration. When we put the attitude sensor on a movable body like an UAV (Unmanned Aerial Vehicle), this kind of error is fatal for the sensor. So, we apply the extended kalman filter algorithm to reduce the estimation error. Firstly, we derive the process model for the kalman filter which is based on quaternion kinematics. Secondly, we design the extended kalman filter by using the process model. Lastly, we show the simulation and experiment result of the estimation algorithm. 1.‚Í ‚ ¶ ‚ß ‚É ‹ß "N,‹ó ŽB‚â "_-ò ŽU•z,ƒŠ ƒ‚ •[ ƒgƒZ ƒ" ƒVƒ" ƒO‚AE‚¢ ‚Á‚½-l•X‚È•ê-Ê ‚É ‚¨‚¢‚¨‚¢ ‚Ä,‚» ‚ê ‚Ü‚Å-p‚¢ ‚ç ‚ê ‚Ä ‚« ‚½-L •l •q ‹ó ‹@‚É'ã ‚í ‚è-³ •l •q ‹ó ‹@‚ª Šˆ-Šˆ-p‚³ ‚ê ‚é ‚± ‚AE‚ª '½ ‚-‚È ‚Á‚Ä‚¨‚è‚Á‚Ä‚¨‚è'•-Ú‚ð •W‚ß ‚Ä‚¢ ‚é.‚± ‚ê ‚ç ‚Ì-³ •l •q ‹ó ‹@‚Í '• •Ì ‚µ ‚ÄUAV(Unmanned Aerial Vehicle) ‚âMAV(Micro Air Vehicle)‚AE OEÄ ‚Î ‚ê,-L •l •q ‹ó ‹@‚É "ä ‚× ‚Ä ƒRƒX ƒg‚âˆÀƒg‚âˆÀ'S •«,-˜ •Ö•« ‚É-D ‚ê ‚Ä‚¢ ‚é ‚AE‚¢ ‚¤ ‚± ‚AE‚© ‚ç,•«-ˆ"I ‚É ‚Í ŠÄŽ‹,"_ OEŸ,-A'-‚AE‚¢ ‚Á‚½-l •X‚È ƒ~ ƒb ƒVƒ ‡ ƒ"‚Ö ‚Ì ‰ž-p ‚ª Šú 'Ò ‚³ ‚ê ‚Ä ‚¢ ‚é.UAV/MAV‰^-p‚Ì '• Šú ŽÀOE»‚Ì ‚½‚ß ‚É •ae ‚è ‰z‚¦ ‚é ‚× ‚« ‚¢ ‚-‚‚© ‚Ì ‹Z •p "I ‰Û 'è ‚ª ‚ ‚é ‚ª,‚» ‚Ì ' † ‚Å ‚à •Å ‚à 'å ‚« ‚È ‚à ‚Ì ‚AE‚µ ‚Ä Ž©-¥ • § OEä‹Z •p ‚Ì Šm-§‚ª ‚ ‚é.‚à ‚µ,Ž©-¥• § OEä‹Z •p ‚ª Šm-§‚³ ‚ê ‚Ä ‚¢ ‚È ‚¯ ‚ê ‚Î,‰^-p‚Í ƒI ƒy ƒOE •[ ƒ^‚Ì-³ •ü '€ •c ‚É-Š‚é ‚µ ‚© ‚È ‚-,ƒI ƒy ƒOE•[ ƒ^‚Ì •‰'S ‚à OEy"÷ ‚Å ‚Í ‚È ‚¢ ‚½‚ß ‚Å ‚ ‚é.ˆÈ •ã ‚Ì ‚ae ‚¤ ‚È-•-R ‚© ‚ç,UAV/MAV‚Ì Ž©-¥• § OEä‚É ŠÖ‚· ‚é OE¤‹ † ‚ª •¢ ŠE ' † ‚Å•s ‚í ‚ê ‚Ä ‚« ‚½[1•z[2].-{ OE¤‹ † ƒOƒ‹ •[ ƒv ‚Å ‚Í,1999"N ‚ae ‚è •¬ OE^UAV‚̈ê OE^UAV‚Ìˆê ‚Â ‚Å ‚ ‚é •¬ OE^-³ •l ƒw ƒŠƒRƒv ƒ^‚Ì Ž©-¥ • § OEä‚É ŠÖ‚· ‚é OE¤‹ † ‚ð •s ‚Á‚Ä‚¨‚è‚Á‚Ä‚¨‚è,ƒw ƒŠƒRƒv ƒ^ ‚Ì ƒ_ ƒCƒi ƒ~ ƒNƒX‚ð •\ OE»‚µ ‚½•ü OE` ƒ‚ ƒf ƒ‹ ‚ð "¯ 'è "I ‚É ‹• ‚ß,"¾ ‚ç ‚ê ‚½ ƒ‚ ƒf ƒ‹ ‚É 'Î ‚µ ‚Ä•Å "K • § OEä-•˜_ ‚â ƒ• ƒo ƒX ƒg• § OEä‚È ‚Ç ‚Ì Še Ží ƒ‚ ƒf ƒ‹ ƒx •[ ƒX•ÝOEv-@‚É ‚ae ‚è • § OEäŠí ‚ð •ÝOEv‚· ‚é ‚AE‚¢ ‚¤ "AEŽ©‚Ì ƒAƒv ƒ• •[ ƒ` ‚É ‚ae ‚Á‚Ä,‰ä ‚ª •' ‚Å ‚Í •‰‚AE‚È ‚é •d-Ê ‚ª-ñ10[kg]'ö "x ‚Ì •¬ OE^-³ •l ƒw ƒŠƒRƒv ƒ^‚Ì Ž©-¥• § OEä‚É•¬ OE÷ ‚ð Žû ‚ß ‚Ä‚¢ ‚é[3]•`[5].‚» ‚µ ‚ÄOE»•Ý ‚Å‚Í,‚³ ‚ç ‚É•¬ OE^‚È ‹@'Ì ‚Ì Ž©-¥• § OEä‚ð-ÚŽw ‚µ ‚Ä‚¢ ‚é. UAV/MAV‚É OEÀ ‚ç ‚¸,ˆê "Ê "I ‚È•q ‹ó ‹@‚Ì • § OEä‚Í •q-@ •E-U "± •E (Žp •¨)• § OEä‚AE‚¢ ‚¤ ŽO‚Â‚Ì ƒt ƒF•[ ƒY‚© ‚ç •¬ ‚è-§‚Á‚Ä‚¢ ‚é ‚ª,•¬ OE^ ‹@'Ì ‚Ì Ž©-¥ • § OEä‚ð •l ‚¦ ‚é •Û,‚± ‚Ì ' † ‚Å ‚à•q-@‚ª "Á ‚É•d-v‚AE‚È ‚é. •¬ OE^‹@'Ì ‚Ì •ê • ‡,"‹ •Ú•d-Ê ‚Ì • § OEÀ ‚© ‚ç •‚ •¸ "x ‚Å•d ‚¢ ƒZ ƒ"ƒT ‚Í "‹ •Ú ‚Å ‚« ‚È ‚¢ ‚½‚ß,•¡ •" ‚Ì 'á •¸ "x ‚ÅOEy‚¢ ƒZ ƒ"ƒT ‚ð "‹ •Ú ‚µ,•â •³ •q-@‰‰ ŽZ ‚ð ŽÀ'• ‚· ‚é ‚± ‚AE‚Å•‚ •¸ "x ‚Ì •q-@ƒf •[ ƒ^ ‚ð "¾‚é ‚± ‚AE‚ª •K-v ‚AE‚È ‚é ‚½‚ß ‚Å ‚ ‚é.‚» ‚Ì ‚ae ‚¤ ‚È ' †,•ae •s OE¤‹ † ‚Å ‚Í OEy-Ê ‚Å'á •¸ "x ‚Ì ƒZ ƒ"ƒT ƒf •[ ƒ^ ‚© ‚ç ‹@'Ì ‚Ì Žp•¨ •E •û ˆÊ ‚ð •" 'è ‚· ‚é ‚½‚ß ‚ÌMARG (Magnetic, Angular Rate and Gravity)ƒZ ƒ"ƒT ‚ð ŠJ "-‚µ[6], ‚± ‚Ì ƒZ ƒ"ƒT ‚ð-p‚¢ ‚Ä-ñ5[kg]'ö "x ‚Ì ‹@'Ì ‚Ì ƒz ƒo ƒŠƒ" ƒO• § OEä‚É •¬ OE÷‚µ ‚Ä‚¢ ‚é[7].‚µ ‚© ‚µ,‚± ‚ÌMARGƒZ ƒ"ƒT ‚É ‚Í ‰Á '¬ "x ŠÂ‹« ‰º ‚Å'å ‚« ‚È Žp •¨ OEë•· ‚ð • ¶ ‚ ¶ ‚Ä ‚µ ‚Ü‚¤ ‚AE‚¢ ‚¤-â 'è ‚ª ‚ ‚é ‚± ‚AE‚ª-¾‚ç ‚© ‚É ‚È ‚Á‚Ä‚¢ ‚é.•¡ ‰ñ‚Í,‚» ‚Ì ‚ae ‚¤ ‚ÈŽp •¨ OEë•· ‚Ì 'á OE¸‰»‚ð-ÚŽw ‚µ,ƒN ƒH•[ ƒ^ƒj ƒI ƒ" ‚AE Šg '£ ƒJ ƒ‹ ƒ} ƒ" ƒt ƒBƒ‹ ƒ^ ‚ð-p ‚¢ ‚½Žp•¨ •" 'è ƒA ƒ‹ ƒSƒŠƒYƒ€ ‚ð'ñ ˆÄ‚· ‚é.ƒN ƒH•[ ƒ^ƒj ƒI ƒ" ‚AE Šg '£ ƒJ ƒ‹ ƒ} ƒ" ƒt ƒBƒ‹ ƒ^ ‚ð-p ‚¢ ‚½Žp •¨ •" 'è ƒAƒ‹ ƒS ƒŠƒYƒ€ ‚É ŠÖ ‚µ ‚Ä‚Í,‰ß ‹Ž ‚É ‚àOE¤‹ †[8][9] ‚ª ‚ ‚è,‚Ü ‚½,‰Á '¬ "x ŠÂ‹« ‰º‚ÅŽp•¨ OEë•· ‚ð • ¶ ‚ ¶ ‚É ‚-‚¢ Žs "Ì ‚Ì ƒZ ƒ"ƒT[10]‚ª ‚ ‚é.‚µ ‚© ‚µ,ƒZ ƒ"ƒT[10]‚É ŠÖ ‚µ ‚Ä ‚Í,‚» ‚Ì ƒAƒ‹ ƒS ƒŠƒYƒ€ ‚Ì •Ú •× ‚È •ñ •• ‚ª ‚È ‚¢ •ã ‚É,ƒZ ƒ" ƒT•d-Ê ‚ª •d ‚¢ ‚½‚ß •¬ OE^-³ •l ƒw ƒŠƒRƒv ƒ^"™‚Ö‚Ì "K-p‚Í "ï ‚µ ‚¢.• ¶ OE£[8]‚É ŠÖ‚µ ‚Ä‚Í,"Á ‚ɉÁ '¬ "x ŠÂ‹« ‰º‚Å ‚Ì Žg-p‚É ŠÖ‚· ‚é OE¾ ‹y ‚Í ‚È ‚-,ˆÚ "®'Ì ‚Ö ‚Ì "K-p‚Ì ‰Â "Û ‚É‚Â ‚¢ ‚Ä ‚Í-¾‚ç ‚© ‚AE‚È ‚Á‚Ä‚¢ ‚È ‚¢.• ¶ OE£[9]‚Å ‚Í,‰Á '¬ "x ŠÂ‹« ‰º‚¨‚ae‰º‚¨‚ae ‚ÑŽ¥ ‹C ŠO-•‰º‚Å ‚Ì Žp•¨ •" 'è ‚Ì •‚ •¸ "x ‰»‚Ì •û-@‚É ‚ ‚¢ ‚Ä•q