Pitching angle estimation for a flapping-wing robot using a rate gyro sensor and an acceleration sensor. T. Miyazaki, Y. Higashi and H. Kimura (Kyoto

Size: px

Start display at page:

Download "Pitching angle estimation for a flapping-wing robot using a rate gyro sensor and an acceleration sensor. T. Miyazaki, Y. Higashi and H. Kimura (Kyoto"

ゆずささわまつ
4 years ago
Views:

1 Pitchig agle estimatio for a pig-wig robot usig a rate o sesor ad a acceleratio sesor. T. Miyazaki, Y. Higashi ad H. Kimura (Kyoto Istitute of Techology) Abstract This paper presets the estimatio method of the pitch agle for a pig-wig robot which has a terrible oise from actuatio of the pig motio. Our pig-wig robot has a D rate o sesor ad a 3D acceleratio sesor. Although we ca calculate pitch agles by each sesor, the pitch agle calculated from the rate o sesor drifts agaist the collect agle. Ad the pitch agle calculated from the acceleratio sesor cotais oises of iertia. For solvig these problems, i our estimatio method, the pitch agle is calculated usig the digital low-pass filtered value measured by the rate o sesor ad the digital high-pass filtered value measured by the acceleratio sesor. Ad this method icludes calibratio of voltage fluctuatio i the rate o sesor caused by chagig pig frequecy. Key Words: Attitude estimatio, Flappig-wig, Robot A/D Baerveldt ) Pogsak ) 3) (7) Sigh ) 5) Baerveldt ) IIR Sesor uit IMU 5 Degrees of Freedom] Cotroller uit [Mii Axis Motor Drive] Flappig motor y roll x pitch yaw z Tail rotor Fig. : The sesor uit (the D rate o ad the 3D acceleratio sesor), the cotroller uit (microcomputer ad D motor driver), the pig-wig robot, ad defiitio of axes. CCP Fig. 3 SparkFu

2 Electroics 社製の IMU 5 Degrees of Freedom を用いコントローラユニットはマイクロコンピュータ SH77 と軸のモータドライバによって構成された HiBot 社製の Mii -Axes Motor Drive を使用している 3 計測装置 Carbo pipe of mm i diameter Woode stick Weight Rotary Ecoders Rotary ecoder measures the pig frequecy. Flappig Robot 則により離散積分して θ = の様に取得する実際には式 () における v が丸め誤差を含むため式 () の積分によってそれが蓄積され θ は徐々にドリフトする. 加速度センサ加速度センサによる角度 θacc は Fig. より ( acc ) x θacc = arcsi g Fig. : The measuremet istrumet for the pigwig robot. 羽ばたきロボットの飛翔状態を計測するため Fig. に示すような計測装置を製作したこれは羽ばたきロボットを球表面に拘束することで長時間の飛翔観測を可能にする装置であるつのロータリエンコーダにより羽ばたきロボットの飛翔速度高度ピッチ角ヨー角を計測できる本稿では Fig. 左下の拡大図に示すピッチ角計測エンコーダと羽ばたき周波数計測エンコーダを用いてピッチ角 θmsr と羽ばたき周波数を測定する尚羽ばたきロボットは羽ばたきモータへの PWM 信号のデューティ比 γ を操作することで羽ばたき周波数を変化させる両者の関係を Fig.3 に示す (3).. acc x Rotary ecoder measures the pitch agle. γ [%] Fig. 3: The relatioship betwee the duty ratio (γ ) ad the pig frequecy ( ). () k= の様に算出するここで x acc は加速度センサから計測した x 方向加速度 g は重力加速度である軸の定義は Fig. 右上参照 Closeup ) Ts ( θ k θ k 姿勢角推定における問題点. レートジャイロ使用しているレートジャイロでは時刻における角速度 θ が以下の様に計測されている θ =K (v v ) () ここで K は電圧を角速度に変換する係数であり本稿で用いるレートジャイロ IDG-3 では K = 5[deg/s/V] であるまた v は時刻におけるレートジャイロの出力電圧 v は初期出力電圧であり初期状態は静止しているものとするサンプリング周期が Ts のとき時刻における角度 θ はこれを台形 acc g Fig. : The x-axis acceleratio(x acc ) ad the pitch agle(θacc ). θ の場合 θ に表れる羽ばたきノイズは式 () の積分によって吸収されるが θacc では x acc に表れるノイズが逆正弦関数によって伝達される付近ではやや軽減されるが ±9 に近づくとノイズは拡大される従って θacc に慣性ノイズが表れ動的な状態における推定精度は悪化する.3 羽ばたき運動によるノイズ Fig.5 は既知の入力であるピッチ角 θmsr と羽ばたき周波数 3 章参照を与えたときの上段レートジャイロから算出したピッチ角 θ 中段加速度センサから算出したピッチ角 θacc 及び下段 θacc をローパスフィルタに通した出力を示している約秒までは羽ばたかない状態で胴体をピッチングさせ 5 秒前後で並進運動させた秒付近から羽ばたかせて 5 3 秒で再びピッチングさせた Fig.5 上段ではレートジャイロの値は徐々にドリフトしまた中段より加速度センサの値が並進運動と羽ばたき運動のノイズの影響を大きく受けていることがわかる特に羽ばたきによるノイズは振幅が大きく姿勢角制御に直接使用することはできないそこでこの羽ばたきノイズを除去するために MATLAB を用いて高域遮断周波数を Hz に設計した次の FIR (Fiite Impulse Respose) ローパスフィルタを用いたその結果が Fig.5 下段であるが羽ばたきノイズを除去できている代わりに約. 秒の位相遅れが生じている 5 ピッチ角推定 5. 次の IIR フィルタサンプリング周期を Ts 時刻における入力を x としたときデジタルローパスフィルタの出力 yl を次の IIR (Ifiite Impulse Respose) フィルタを用いて以下の様に定義する ( ) L L yl = y r x y = LPF (x ) ()

3 y L Agle [rad] Agle [rad] Agle [rad] θ Pitchig Traslatio Pitchig ad Flappig θ acc FIR(θ acc ) Fig. 5: The is a referece iput of the pitch agle. The is the pig frequecy. Top: A pitch agle calculated from the rate o sesor (θ ). Middle: A pitch agle calculated from the acceleratio sesor (θ acc ). Bottom: A output of the FIR lowpass filter of θ acc. r f cut r = exp ( πf cut ) T s (5) () f c f c f cut (f cut T s ) () ) f cut T s () 7) () z (9) y L = rx ( r)y L (7) Y L (z) = rx(z) ( r)z Y L (z) () H L (z) = Y L(z) X(z) = r ( r)z (9) X(z) Y L (z) x y L z y H () y H = x y L = HPF (x ) () H H (z) = H L (z) = ( r) ( r)z ( r)z () (9) () Fig.(a),(b) X(z) r ( - r) ( - r) X(z) ( - r) z - Y(z) (a) First order IIR low-pass filter. z - ( - r) (b) First order IIR high-pass filter. Y(z) Fig. : (a) The block diagram of the first order IIR low-pass filter expressed i eq.(9). (b) The block diagram of the first order IIR high-pass filter expressed i eq.() ) = HPF (θ ) LPF (θ acc ) () HPF() LPF() () () Fig.7 G A (s) G G (s) G A (s) = G G (s) = () HPF() LPF() H H (z) Hacc L (z) H H (z) Hacc L (z) = (3) (9) () () r Fig. () r f cut. [Hz] f cut 3

4 .. x (t) plat. (t) Itegrated aalog filters Acceleratio sesor G A (s) Rate o sesor G G (s).. x acc (t) t Samplig. (t) t Agular trasformatio.. x acc. Arcsie Itegratio acc Digital filters IIR low-pass filter H L (z) IIR high-pass filter H H (z) acc LPF( ) HPF( ) The estimated pitch agle Fig. 7: Overview of the pitch agle estimatio system usig IIR filters. Agle [rad] Agle [rad] HPF(θ ) LPF(θ acc ) est Fig. : Estimatio results of the pitch agle. The is a referece iput of the pitch agle. The is the pig frequecy. Top: Outputs of the highpass filter of the θ, ad the low-pass filter of the θ acc. Bottom: Calcurated pitch agle from eq.(). [Hz].3[Hz].[Hz] Fig. Fig.5 Fig. () LPF(θ ) HPF(θ acc ) Fig Fig.9 () θ HPF(θ ) θ AVR( θ ) AVR() x N y A y A = N N k= N x k = AVR(x ) () N Fig.9 N = 5Hz Fig.9 θ HPF(θ ) θ AVR( θ ) Mii - Axes Motor Drive

5 Agle [rad] Agle [rad] Agular Velocity [deg/s] θ HPF(θ ) AVR(. θ ). θ Fig. 9: The is the pitch agle measured from the measuremet istrumet. The is the pig frequecy. Top: The estimated pitch agle ( ). It has offsets while the pig-wig robot s. Middle: The pitch agle measured by the rate o (θ ) icludes a rapid drift. Bottom: The pitch agular velocity measured by the rate o ( θ ) icludes offsets correspodig to the pig frequecy. v () v γ γ Fig.3 v v = [vi ] Fig. <Step > Update by LPF( v ). <Step 3> Switchig the offset voltage. * K v v j v v v v 3 v v 5 v v 7 v v 9 v Fig. : The algorithm has 3 steps. <Step > Check the duty ratio. Case 9. Case 7. Case. 3. j γ j { γ ( γ j = < ) (γ (5) = ) x x x = max { Z x} () Z (5) PWM γ. j j v (7) v = [ v i ] = [δij LPF (v )] (7) δ { (i = j) δ ij = () (i j) (7) v () γ f cut.5 [Hz] vi = v 3., j v θ est θ = k ( v vj ) (9) θ = T ( ) s θ i θ i () i= = HPF (θ ) LPF (θ acc ) () 3 T s Figs. 3 () () Fig. Fig. Fig.3 7 Fig.3 Fig.

6 Agle [rad] Agle [rad] * Fig. : First experimetal results of the calibratio. Top: The pitch agle calculated from eq.(). Bottom: The pitch agle calculated from eq.(). Agle [rad] Agle [rad] * Fig. 3: Seveth experimetal results of the calibratio. Top: The pitch agle calculated from eq.(). Bottom: The pitch agle calculated from eq.(). Agle [rad] Agle [rad] * Fig. : Secod experimetal results of the calibratio. Top: The pitch agle calculated from eq.(). Bottom: The pitch agle calculated from eq.(). IIR ) ) A. Baerveldt ad R. Klag: A Low-cost ad Low-weight Attitude Estimatio System for a Autoomous Helicopter, Proc. of IEEE INES 97, 39/395 (997) ) L. Pogsak, M. Okada, T. Siohara ad Y. Nakamura: Attitude Estimatio by Compesatig Gravity Directio,, A3 (3) 3) Vol.39 No. 9/33 (7) ) S.P.N. Sigh ad K.J. Waldro: Attitude Estimatio for Dyamic Legged Locomotio Usig Rage ad Iertial Sesors, Proc. of ICRA 5, 75/ (5) 5) ) sh okada/tec/ digital filter.html 7) 7/5 (99) )

ディジタル信号処理

ディジタル信号処理ディジタルフィルタの設計法. 逆フィルター. 直線位相 FIR フィルタの設計. 窓関数法による FIR フィルタの設計.5 時間領域での FIR フィルタの設計 3. アナログフィルタを基にしたディジタル IIR フィルタの設計法 I 4. アナログフィルタを基にしたディジタル IIR フィルタの設計法 II 5. 双次フィルタ LI 離散時間システムの基礎式の証明 [ ] 4. ] [ ]*