‐拡張カルマンフィルタによる姿勢推定‐

平成25年度 修士学位論文

農業ロボット用モーションセンサの開発

‐拡張カルマンフィルタによる姿勢推定‐

平成 26年 3月

三重大学大学院 生物資源学研究科 共生環境学専攻

応用環境情報学教育研究分野

柳瀬宣彦

緒言 ... 1

農業ロボットの自動化 ... 2

農業ロボットの課題 ... 3

研究目的 ... 4

第一章 慣性センサ 1.1 供試センサ ... 5

1.2 センサの動作原理 ... 7

1.3 小型マイコンボード ... 10

1.4 LOGICALPRODUCT製9軸ワイヤレスモーションセンサ ... 14

1.5 加振実験 ... 17

第二章 姿勢角計測 2.1 オイラー角とクォータニオンによる姿勢表現 ... 24

2.2 姿勢計測 ... 28

2.3 6自由度運動計測 ... 32

第三章 加速度環境下における姿勢推定 3.1 拡張カルマンフィルタ（EKF）による姿勢推定 ... 38

3.2 ローパスフィルタ ... 42

3.3 シミュレーションによるEKFの検証 ... 43

3.4 加速度環境下における姿勢推定 ... 45

結言 ... 54

展望 ... 55

謝辞 ... 56

参考文献 ... 57

付録 ... 59

緒言

わが国の食料生産基盤は脆弱であり，食料自給率は40パーセントに過ぎず，先進諸国 の中で最低の数値である．日本政府は2020年まで目標食料自給率を50パーセントに設 定する．一方で総農家数は1990年には482万戸であったのに対して，2010年間で54 パーセントまで激減した．加えて，農村地域では，若年層の流出により，農業従事者の 平均年齢は65・8歳になり，労働不足は深刻さを増している．さらに，農産物の輸入自 由化が進む中で，国際競争力を確保するために，一層の品質の向上や生産のコストの削 減を図ることが課題となっている．こうした状況の中、国内農業の構造改革と併せて，

農業生産の新たな技術形成に対して革新的な展開が求められている．このような背景か ら， 農業経営の経済的な採算性に適合するようなロボット化を含めた超省力技術の開 発が， 日本農業を持続的に維持・発展させる上で必須である¹⁾．

農業ロボットの自動化

農業ロボットとは，「農業および生物生産に関わるもので，アームのように複数の自 由度の機構をもつ機械システム，高度な感覚機能，判断機能を持つ自動機械，および従 来系の農業機械でも自律移動できるものなどを農業ロボットと呼ぶ」ことである²⁾．現 在，ロボットトラクタや収穫ロボットなどさまざまな農業ロボットが開発されている．

これら農業ロボットを大きく分けるとすると3つに分類することができる．既存の機械 を改良して無人運転する「車両系ロボット」，機械化されていない作業を自動化する「マ ニピュレータ系ロボット」，農業施設自体を自動化させ植物工場などを実現する大規模 なものである．車両系ロボットは，トラクタや田植機など従来農業機械の走行部，作業 部を制御できるように改造し，その上に無人走行や作業を支える航法センサやコントロ ーラを装備したものである．1990年代にはロボットトラクタの基礎が築かれており，

ほぼ実用化のための技術的問題は解決されている³⁾．マニピュレータ系ロボットは，対象 物の検出を行う視覚部，作業を行うマニピュレータ部（アーム部），これに付加したハン ド（エンドエフェクタ）部から上半身が構成される．また，下半身側は、移動機構と制御 コントローラ，動力源や収穫物の収容部などから構成される．イチゴ⁴⁾やナス⁵⁾などの収 穫ロボットや苗を接ぎ木するためのロボット⁶⁾などがある．

これら農業ロボットの自動化に必要な機能をまとめると，ロボットの自己位置や姿勢 角などの状態を把握すること，目の前の障害物や進路など外界の情報を認識すること，

またこれらの情報からどのように行動すればよいかを決定する判断・制御機能を有する ことが挙げられる．

農業ロボットの課題

今までにトラクタによる畑作や，田植機などの農業ロボットが開発されているが，一般 的に利用されるには未だ課題が多く残る．課題はいくつかあるが，ロボットの「コスト」

と「精度」が最大の課題として挙げられる．農業ロボットに要求される精度は船舶・航 空機の自動操縦などの他産業に要求される精度に比べ高く，そのため高精度ではあるが 高価なセンサが必要となる．例えば，RTK-GPS を導入する場合，その価格は250 万円 以上に及ぶ．現在，日本の農家で1000万円以上の収入がある農家割合はわずか6.3％と いうことを考えると，いくら高機能で高性能な農業ロボットを開発しても，このコスト の増大は実用化には大きな障害となる⁷⁾．また，GPSは衛星から電波を受信することで 位置を割り出すため，基本的に屋外でのみの計測に限られる．屋内や遮蔽物がある環境 では，GPSの精度が低下するためGPSは使用できない．GPSに頼らず，「コスト」と

「精度」を考慮したセンサシステムの開発が必要である． そこで近年，MEMS 技術

（Micro Electro Mechanical Sysytems Technology）により著しく小型化，低価格化が進ん でいる慣性センサを利用した研究が行われている．

研究目的

以上の背景より，本研究は農業ロボットの自動化の課題となっている「コスト」と「精 度」の課題を解決するために，超小型，薄型であり，低消費電力な慣性センサよる農業 ロボットの自動化に必要不可欠な位置・姿勢角を計測し，実用的なセンサフュージョン システムを構築することを目的とした．対象のロボットは，施設園芸内で作業する収穫 ロボットや遮蔽物がある環境で作業する防除ロボットであることから，屋内環境で作業 するロボットを想定しており，地磁気センサや GPS を利用せず，加速度・ジャイロセ ンサのみでの位置・姿勢計測を目指す．

位置と姿勢情報は，理論的には加速度・ジャイロセンサから得られる加速度・角速度値 を積分し求められる．しかし，角速度を積分演算した場合，ジャイロセンサの出力に含 まれるバイアス誤差が蓄積し，時間経過にともない精度が低下する．重力加速度からの 演算においては，方位情報が得られない．また動的環境下での加速度センサが出力する 加速度は，回転運動の際生じる加速度成分を持つため，単純な積分計算では正確な位置を 把握することは難しい．位置計測に必要な並進加速度のみを算出するには，その時間にお ける対象物の姿勢を計測し，余分な加速度成分を取り除く必要がある．そのため，リアル タイムでの姿勢計測の精度が重要となる．本研究では，より精度の高い姿勢情報を算出す るため，クォータニオンを用いた姿勢演算アルゴリズムを導入し，誤差推定を行うデジタ ルフィルタを加えた． 本稿では，静的運動における姿勢計測と加速度環境下での姿勢推 定の結果を報告する．

1 ^{慣性センサ}

1.1

供試センサ

供 試 セ ン サ は ， InvenSense 社製 3 軸 ジ ャイ ロ セ ン サ モ ジ ュール ITG3200 と

ANALOGDEVICES社製3軸加速度センサモジュールADXL345を使用した（図1-1，表

1-1）．ADXL345は小型，薄型，低消費電力の3軸加速度センサであり，最大± の測

定範囲で高分解能（13ビット）の加速度計測が可能である．デジタル出力データは 16 ビットの「2の補数」フォーマットで，SPI（3または4線）あるいはI²Cのデジタル・

インターフェースを介してアクセスできる．ADXL345 は，測定範囲に応じて分解能が 分かれており，本研究では測定範囲±2 ，分解能10 [bit]に設定した．ADXL345は，

重力の静的加速度を計測することができると同時に，動き，衝撃あるいは振動のような 動的加速度も計測できる．傾き検出に使用した場合は，1.0°以下の傾き変化を測定する ことが可能である．

ITG3200はADXL345と同様に小型，薄型，低消費電力のジャイロセンサであり，16

ビット分解能ADコンバータが内蔵している．感度は約13 [LSB/(degree/s)]，測定範囲 は±2000 [degree/s]であり，インターフェースはI²C通信が利用できる．本研究では，通 信方式は，加速度・ジャイロセンサともにI²C通信を選択している．これはSPI通信よ りI²C通信の方が配線の本数が少なく，接続が比較的容易であることと，ジャイロセン サの通信方式がI²C通信のみであるためである．また，両センサともに，出力がデジタ ル値であるため，加速度センサは出力単位を [m/s²]に，ジャイロセンサは [degree/s]に 変換する必要がある．I²C通信の簡単な仕組みは付録に記述した．

図1-1 ジャイロセンサ（写真上），加速度センサ（写真下）

表1-1 慣性センサの仕様

センサ 名称

加速度センサ ADXL345

ジャイロセンサ ITG3200

サイズ 4×4×0.9mm 3×5×1mm

測定範囲 ±2,±4,±8,±16g ±2000degee/s

分解能 10,11,12,13bit 16bit

電源 2-3.6V 40-145μA 2.1-3.6V 6.5mA

1.2 センサの動作原理

1.2.1

加速度センサ

加速度センサの加速度検出の原理を記す．加速度は，式1-1のニュートンの第二法則 と式1-2のフックの法則から求めることができる．式1-1の は慣性力 [N]，mは質量 [kg]，aは慣性加速度 [m/s²]である．また式1-2の は復元力 [N]，kはばね定数 [N/m]，

xは変位 [m]である．

1-1

1-2

式1-1と式1-2から慣性力と復元力が釣り合うとき，式1-3が成り立ち，変位から慣性 加速度が求められる．筺体の加速度aext [m/s²]と慣性加速度は逆向きである（図1-2）．

1-3

図1-2 加速度センサの基本モデル

実用化されている加速度センサの多くは，ピエゾ抵抗型か静電容量型である．本研究 に用いた加速度センサADXL345は静電容量型であるため，ここでは静電容量型3軸加 速度センサの構造を記す．静電容量型3軸加速度センサは，半導体技術とマイクロマシ ニング技術を利用して作られている．図1-3は，静電容量式センサの断面図である．Si 基板と対向するように，固定基板が（ガラス基板）が Si 基板に接合されている．固定 基板の下面には，図1-4にあるように5個の静電容量が形成されている．重錘体に加速 度が作用すると，重錘体は加速度の方向に変化する．図1-4の電極配置で静電容量の変 化は，表 1-2 のようになる． X 軸方向の加速度を検出するためには，静電容量 C1 と C2の差を求め，Y軸方向の加速度を検出するためには，静電容量C3とC4の差を求め ればよく，Z軸方向の加速度を検出するためには，C5の値を検出する⁸⁾．1軸のみを考 えると，静電容量の変化△C [F]は式1-4となる． は誘電率 [F/m]，Sは極板面積 [m²]，

x k

ma a_ext

dは極板間距離 [m]の初期値である．式1-4より，加速度aを移項することにより加速 度を計測することできる．

1-4

図1-3 静電容量型センサの断面図

図1-4 電極配置パターン

表1-2 静電容量の変化

X-axis Y-axis Z-axis

C1 C2 C3 C4 C5

Ax ＋ － 0 0 0

Ay 0 0 ＋ － 0

Az ＋ ＋ ＋ ＋ ＋

Si

基板 変位電圧

固定電圧

重錘体

X

Y

C3

C2 C4

C1 C5

1.2.2

ジャイロセンサ

ジャイロセンサの角速度検出の原理を記す．ジャイロセンサは，センサ本体の回転量 を電気信号に変換し，角速度として算出するデバイスである．ITG3200の構造は，セン サ内部の振動を利用する「振動式ジャイロ」である．

図1-5 振動ジャイロの力学モデル

振動ジャイロの原理は，「速度をもったものに角速度が印加されると速度と角速度が ともに直交する方向にコリオリの力が生じる」という，コリオリの力を利用している．

振動ジャイロは速度とコリオリの力を弾性帯の振動の中で生み出しているので，回転体 のない角度センサとなっている．図1-5は振動ジャイロの力学モデルである．いま，質

量m [kg]がX軸方向に振動速度V [m/s]で振動している状態で角速度 [rad/s]が印加され

ると，コリオリの力がY軸方向に発生し振動が励起される．この振動振幅は角速度に 比例するので振幅を検出することで，式1-5から入力加速度を知ることができる⁹⁾．

1-5

また ITG3200 の振動素子は，水晶が使われている．水晶は単結晶であるため，特性

の経時変化が少なく，品質のばらつきも抑えられ，水晶の圧電性を利用した駆動/検出 機構なので，構成がシンプルである．水晶は圧電体であるので電圧を印加するだけで振 動させることができ，駆動インピーダンスを下がられる．また，水晶は熱膨張と弾性の 温度変化が相殺するので，温度特性が良好な特徴をもつ¹⁰⁾．

Fc : コリオリ の力

m : 質量

V : 振動速度

ω ：角速度

X

Y

1.3

小型マイコンボード

データ出力は，小型マイコンボードArduino（ATMGA328P）を用いた（図1-6）．Arduino とは，専用のAVRマイコンボードとソフトウェアが一体となった環境開発のことを指 す．ハードウェアには，電源レギュレータ ，AD コンバータ ，USB ジャック ，アナ ログ入出力6CH，デジタル入出力13CHなどが備え付けられている．ソフトウェアは，

Arduino専用のIDE（Integrated Development Environment）をインターネットからファイ

ルをダウンロードして用いる．プログラム言語は，Processing言語をもとにした言語を 使い，Arduinoボードで実行するスケッチを書くことができる¹¹⁾．プログラムの基本的 な流れは，次の通りである．①コンピュータの USB ポートにArduino ボードを接続す る．②IDE の上でスケッチを書く．③スケッチをボードへ USB 経由でアップロードす る．④スケッチが実行される．センサとArduinoの接続は，専用のユニバーサル基盤の 上にセンサを配置し，ピンを上からArduinoに差し込むことで簡単に接続できる（図1-7）．

データ収録は，図1-8ようにUSB接続によるシリアル通信により出力される．

図1-6 Arduino（ATMGA328P）

図1-7 慣性センサモジュール 図1-8 データ収録

1.3.2

マイコンボードとセンサモジュールの接続

センサモジュールとマイコンボード（Arduino）の接続ピンを表1-3に，回路図を図 1-9に示す．加速度センサと角速度センサのSDA，SCLピンをArduinoの4番，5番ピ ンに接続する．加速センサのSDA，SCLピンは，抵抗10 [kΩ]を介して，電源3.3 [V]

に接続する．

表1-3 接続ピン

Arduino Pin ITG3200 Pin ADXL345 Pin

A4 SDA SDA

A5 SCL SCL

GND GND GND

3.3V VDD,VIO VCC, CS,SDA,SCL

図1-9 回路図

1.3.3

出力データの校正

加速度センサADXL345，ジャイロセンサITG3200の出力値を校正する．出力データ は両センサともデジタル値であるため校正後，加速度センサは加速度 [m/s²]に，ジャイ ロセンサは角速度 [degree/s]に変換する．センサを X，Y 平面上に水平に設置させ，静 止状態の加速度，角速度を計測した．表1-4に生データの値を示す．表の値は，静止さ

せてから0 秒から 0.019秒までの一部分である． 加速度センサのX，Y 軸方向の校正

式は式1-6である．式1-6では，まず生データ( ) から平均値( ) を算出する（表

1-5）．次に，生データから算出された平均値を引き，加速度値が静止状態で 0 付近に

なるよう校正する（表1-6）．ADXL345の分解能を1 [G]あたりのデジタル値で表すと，

256 [LSB/1G]であるため，256 で割り，重力加速度[G] を算出してから重力加速度

9.80665[m/s²] をかけることにより，加速度[m/s²] を求める．表1-7は，校正後の加速度

[m/s²] の値である．Z軸方向の加速度の校正式は式1-7である．Z軸方向の加速度は，

センサをX，Y平面上に水平に設置すると重力加速度を出力する．分解能が256 [LSB/1G]

であることから，加速度を 256 付近に校正してから，X，Y 軸方向の加速度と同様に，

加速度 [m/s²]を出力する．角速度の校正式は，式1-8である．加速度の校正と同様にデ ジタル値の生データから平均値を引き，デジタル値を 0 付近になるように校正する．

ITG3200の分解能は14.375 [LSB/(degree/s)]であるため，14.375で割ることにより角速度

[degree/s]が算出できる．

1-6

1-7

1-8

表1-4 デジタル値（生データ）

加速度センサ（Accx, Accy, Accz），角速度センサ(Gyrox, Gyroy, Gyroz)

Time[s] Acc Accy Accz Gyrox Gyroy Gyroz

0 39 4 203 -77 13 7

0.005 41 4 203 -80 16 12

0.009 41 2 203 -77 15 9

0.014 41 4 204 -78 14 11

0.019 40 5 206 -80 15 12

表1-5 デジタル値の平均値

Accx Accy Accz Gyrox Gyroy Gyroz

39.90476 4.127588 203.5228 -78.8036 14.25673 9.384834

表1-6 デジタル値校正

Time[s] Accx Accy Accz Gyrox Gyroy Gyroz

0 -0.90476 -0.12759 255 1.803571 -1.25673 -2.38483 0.005 1.095238 -0.12759 255 -1.19643 1.743271 2.615166 0.009 1.095238 -2.12759 255 1.803571 0.743271 -0.38483 0.014 1.095238 -0.12759 256 0.803571 -0.25673 1.615166 0.019 0.095238 0.872412 258 -1.19643 0.743271 2.615166

表1-7 加速度[m/s²]，角速度[degree/s]

Time[s] Accx Accy Accz Gyrox Gyroy Gyroz

0 -0.03466 -0.00489 9.786624 0.125466 -0.08742 -0.1659 0.005 0.041956 -0.00489 9.786624 -0.08323 0.121271 0.181925 0.009 0.041956 -0.0815 9.786624 0.125466 0.051706 -0.02677 0.014 0.041956 -0.00489 9.824931 0.055901 -0.01786 0.112359 0.019 0.003648 0.03342 9.901546 -0.08323 0.051706 0.181925

1.4 LOGICALPRODUCT

製

軸ワイヤレスモーションセンサ

1.4.1

センサ概要

本研究では，製作した慣性センサモジュールとの精度比較に，㈱LOGICALPRODUCT 製9軸ワイヤレスモーションセンサ（5G/300dps仕様）を用いている（以下LPセンサ とする）． LPセンサは加速度，ジャイロ，地磁気センサを3軸ずつ搭載し，無線を使 用した遠隔計測及びデータロギングを行うことがきるワイヤレスセンサモジュールで ある．単四電池を含めた総重量は約35 [ ]と非常に軽量である（図1-10）．

図1-10 9軸ワイヤレスモーションセンサ

1.4.2

校正

LPセンサに搭載された加速度センサは，㈱ANALOGDEVICES製ADXL325である．

電源電圧3.3Vで使用しているため0 [G]での出力は1.65 [V]である．センサの感度を1G あたり電圧値で表すと190.0 [mV/g]である．加速度センサの電圧出力は12 [bit]でAD変 換される．AD変換値はストレートバイナリであり，フルケースは3.3 [V]である．セン サの出力電圧をVとすると，AD変換値Xaccは式1-9で表される．

1-9 加速度の値をGとすると，変換式は式1-10で表される．

1-10 加速度センサの軸方向は，重力加速度方向へ向けた際に＋1 [G]となる向きが正である．

加速度センサの値は，動かした方向と反対向き抗力が働くため，X軸正方向へセンサを 平行運動させた場合，マイナスの値が表示される．加速度センサの軸方向を図1-11に 示す．

図1-11 加速度センサの軸方向

LPセンサに搭載されているジャイロセンサは㈱EPSON TOYOCOM製XV-3500CBで ある．ジャイロセンサの出力は約 190Hz のローパスフィルタでフィルタリングされ，

オペアンプで5.6倍にされた後にAD変換される．オペアンプはジャイロセンサの基準 電圧出力（Vref=1.35V）を基準として動作しており，この基準電圧とセンサ出力の差を 5.6倍した値がADコンバータへ入力される．0 [degee/s]時のセンサ出力は1.35Vとなる．

センサ感度は0.67 [mV/dps]である．センサの出直電圧をVとすると，AD変換値Xgyro は式1-11となる．

1-11 角速度の値をWとすると，変換式は式1-12となる．

1-12

ジャイロセンサの軸方向は．図1-12のとおりである．

図1-12 ジャイロセンサの軸方向

1.4.3

データ送受信及びアプリケーションソフト

データ送受信装置は，㈱LOGICALPRODUCT製ワイヤレスモーションセンサモジュ ールへのコマンド送受信，計測データ/ファイル受信を行うための専用デバイスを用い

た（図1-13）．2.3 [GHz]帯特定小電力通信を採用し，見通し距離で約50 [m]の無線通信

を行うことができる．データ送受信装置とPCは，USBケーブルで接続され，専用の

LabVIEWアプリケーションを介し，計測及びデータ回収をすることができる．

図1-13 データ送受信装置

図1-14はLabVIEW アプリケーションの実行画面である．アプリケーションは,計測

開始/停止,内蔵メモリへ記録されたファイルの管理や各種設定の変更を行うことがで きる.

図1-14 LabVIEW アプリケーション

1.5

加振実験

1.5.1

実験概要

供試した加速度（ADXL345）・ジャイロセンサ（ITG3200）のセンサ精度を加振実験 により検証した．（独）農研機構食品総合研究所にある加振実験装置（図1-15）お借り し，加振実験を行うことでセンサの動作精度を検証した．この加振実験装置は，図1-16 ように6つの油圧シリンダが（X軸に1つ，Y軸に2つ，Z軸に3つ）備えつけられて おり，シリンダが3軸異なる方向に動作することで，同時に3軸方向に加振運動するこ とができる．また，シリンダの動きに位相差を与えることで，上下，水平方向に簡易的 に，回転運動を与えることができる．加振装置の回転中心は，加振台の上面ではなく，

加振装置の内部にあり，センサを設置した加振台の上面中心とは異なる場所である．振 幅は設定により変化させることができるが，本実験では振幅を変えず一定の値で実験し た．慣性センサの精度比較にLPセンサを用いている．

図1-15加振実験

図1-16 加振実験装置

油圧シリンダ 加振台

X

Y

Z 縦

振 動 横振動

1.5.2

実験条件

慣性センサ（加速度センサADXL345・ジャイロITG3200）の動作精度検証するため，

LP センサの出力と比較した．慣性センサモジュールを加振台の上面の中心に設置し，

USBケーブルを介しPC上にデータ出力した．サンプリング周波数は，慣性センサモジ ュールでは最大の80 [Hz]に，LPセンサでは慣性センサモジュールの周波数に比較的近

い100 [Hz]に設定した．実験項目は，表1-8にあるように X，Z 軸方向に単軸加振，3

軸加振，X軸回転加振の4項目であり，加振装置の周波数を2 ，5，10，20 [Hz]に変化 させて実験をした．加振入力はデータ通信に支障が起きた時には1 [G]ではなく0.5 [G]

で計測を行い，回転加振の周波数は周波数が大きいと加振機の振動が激しくなるため，

10 [Hz]までである．

表1-8 実験条件

実験項目 X，Z単軸，3軸，X軸回転加振 加振入力 1Gまたは0.5G

加振周波数 ・単軸加振 2Hz，5Hz，10Hz，20Hz

・3 軸加振 2Hz，5Hz，10Hz，20Hz

・回転加振 2Hz，5Hz，10Hz

1.5.3 結果及び考察

図1-17にX軸単軸加振，図1-19にZ軸単軸加振，図1-21に3軸加振から得られた 加速度値を，図1-22にX軸回転加振から得られた角速度値をそれぞれ示す．

・図1-17のX軸単軸加振をでは，周波数成分は，図1-18のパワースペクトル密度より，

2，5，10，20 [Hz]すべての加振周波数で捉えられた．振幅のピークでノイズが現れ，若

干波を捉えられていないが，LP センサでも同様なノイズが現れていることから，加振 機装置の動作によるものだと考える．また，高周波になるにつれて，ADXL345 では，

正確な振幅を捉えられず，加振周波数20 [Hz]では顕著である．図1-18のパワースペク トル密度では，加振周波数20 [Hz]の振幅が他の周波数より値が小さい．これは，サン プリング周波数が80 [Hz]では低いため，高周波を捉えきれていないためだと考える．

・図1-19のZ軸単軸加振をみると，ADXL345は，重力加速度をグラフ縦軸正に計測し ており，LPセンサでは，重力加速度を負で計測した．これは，重力加速度の検出方向

がADXL345とLPセンサとでは逆向きであるためである．周波数成分は，X軸単軸加

振と同様に図1-20パワースペクトル密度より，捉えられている．しかし，Z軸方向の 加速度は，X軸の加速度に比べ全体的に波形が歪である．加速度センサの特性上，Z軸 方向の加速度は，X軸方向の加速度に比べ精度が低いことが原因か，もしくは加振機自 体の影響だと考えられる．また，高周波につれ慣性センサでは，正確な振幅を捉えおら ず， 20 [Hz]では顕著である．X軸単軸加振の実験結果と同様に，パワースペクトル密 度にも，加振周波数20 [Hz]の振幅は他の加振周波数より値が小さい．LPセンサとのサ ンプリング周波数の差によるものだと思われる．

・図1-21に3軸方向に同時に加振を与えたときの加速度を示す．X，Z単軸加振に比べ，

全体的に波が歪である．X，Y軸方向の加速度は，ともによく似た波形をしており，精 度が似通っている． 単軸加振と同様に高周波につれ波を正確に捉えきれていない．

・図1-22にX軸回転加振を与えたときの角速度を示す．角速度の周波数成分は，図1-23 のパワースペクトル密度より捉えられている．加速度センサと同様に，LPセンサと比 べると，ITG3200は高周波になるにつれて，正確に振幅を捉えておらず，加振周波数 10Hzでは，波のピークが突になっている．また，LPセンサと比べ若干波が歪になった．

以上まとめると，慣性センサとLPセンサを比較すると，慣性センサは加振周波数が 高周波になるにつれ，波形を捉えきれず，出力した加速度・角速度値が歪なる．これは 慣性センサのサンプリング周波数がLPセンサに比べ小さことが原因していると考える．

図a ADXL345（2Hz,5Hz） 図b LPセンサ（2Hz,5Hz）

図 c ADXL345（10Hz,20Hz） 図d LPセンサ（10Hz,20Hz）

図1-17 X軸単軸加振

図a ADXL345 図b ADXL325

図1-18 パワースペクトル密度（X軸単軸加振）

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

2Hz 5Hz

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

2Hz 5Hz

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

10Hz 20Hz

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

10Hz 20Hz

0 1 2 3 4 5 6 7 8 9 10

0 5 10 15 20 25 30 35 40 PSD[(m/s2)2/Hz]

Frequency[Hz]

2Hz 5Hz 10Hz 20Hz

0 1 2 3 4 5 6 7 8 9 10

0 5 10 15 20 25 30 35 40 PSD[(m/s2)2/Hz]

Frequency[Hz]

2Hz 5Hz 10Hz 20Hz

図a ADXL345（2Hz,5Hz） 図b LPセンサ（2Hz,5Hz）

図 c ADXL345（10Hz,20Hz） 図d LPセンサ（10Hz,20Hz）

図 1-19 Z軸単軸加振

図a ADXL345 図b LPセンサ

図1-20 パワースペクトル密度（Z軸単軸加振）

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

2Hz 5Hz

-15 -10 -5 0 5 10 15

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

2Hz 5Hz

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

10Hz 20Hz

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration[m/s2]

Time[s]

10Hz 20Hz

0 1 2 3 4 5 6 7 8 9 10

0 5 10 15 20 25 30 35 40 PSD[(m/s2)2/Hz]

Frequency [Hz]

2Hz 5Hz 10Hz 20Hz

0 1 2 3 4 5 6 7 8 9 10

0 5 10 15 20 25 30 35 40 PSD[(m/s2)2/Hz]

Frequency [Hz]

2Hz 5Hz 10Hz 20Hz

図a ADXL345（2Hz） 図b LPセンサ（2Hz）

図c ADXL345（5Hz） 図d LPセンサ（5Hz）

図e ADXL345（10Hz） 図f LPセンサ（10Hz）

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

図g ADXL345（20Hz） 図h LPセンサ（20Hz）

図 1-21 3軸加振

図a ITG3200（2Hz,5Hz,10Hz） 図b LPセンサ（2Hz,5Hz,10Hz）

図1-22 X軸回転加振

図a ADXL345 図b LPセンサ

図1-23 パワースペクトル密度（X軸回転加振）

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-20 -15 -10 -5 0 5 10 15 20

0 0.2 0.4 0.6 0.8 1

Acceleration [m/s2]

Time[s]

X-axis Y-axis Z-axis

-80 -60 -40 -20 0 20 40 60 80

0 0.2 0.4 0.6 0.8 1

Angular Velocity [degree/s]

Time[s]

2Hz 5Hz 10Hz

-80 -60 -40 -20 0 20 40 60 80

0 0.2 0.4 0.6 0.8 1

Angular Velocity [degree/s]

Time[s]

2Hz 5Hz 10Hz

0 10 20 30 40 50 60

0 5 10 15 20 25 30 35 40 PSD[(degree/s)2/Hz]

Frequency[Hz]

2Hz 5Hz 10Hz

0 10 20 30 40 50 60

0 5 10 15 20 25 30 35 40 PSD [(degree/s)2/Hz]

Frequency[Hz]

2Hz 5Hz 10Hz

2 ^{姿勢角計測}

2.1

オイラー角とクォータニオンによる姿勢表現

本研究では，一般的に用いられるオイラー角に代わり，計算速度が速く，特異点をもた ないクォータニオン¹²⁾を用いた姿勢計測を行った．クォータニオンは回転軸（ベクトル）

と回転角（スカラー）よりなる4成分で3次元空間を表現する．第二章では，オイラー 角とクォータニオンを使った姿勢表現の比較をする．

2.1.1

座標系

本研究では，絶対座標系と移動座標系の二つの座標系を取り扱う．絶対座標系は，空 間に固定された座標系であり，移動座標系は車両等の車体の重心位置に固定され，絶対 座標系を移動，回転する座標系である．両座標系とも右手系の直交座標系とする．また 移動座標系のX軸をロール角，Y軸をピッチ角，Z軸をヨー角と呼ぶ．絶対座標系から 移動座標系への回転行列をRとする．ここで，絶対座標系の基底ベクトルを ， 移動座標系の基底ベクトルを とすると，以下のような関係で表される．

2-1

2.1.2

オイラー角（タイト・ブライアン角）表現による回転行列

オイラー角は各軸まわりの回転角を組として定義され，回転行列と同様に座標系の関 係を表現する要素である．オイラー角は，回転させる軸の順序により回転行列のパラメ ータが異なる．絶対座標系をZ，Y，X軸の順でそれぞれ ， ， で回転させたとき移 動座標となる角はタイト・ブライアン角¹³⁾と呼ばれ式2-2のように表される．

2-2

＋ ＋ ＋ ＋

式2-2より以下のオイラー角を定義することができる．

2-3

2.1.3 クォータニオンの定式化

クォータニオンqは，回転方向の単位ベクトルnと回転角 の4成分で定義される．ク ォータニオンは図2-1に示す．

図2-1 クォータニオン

2-4 ここで，クォータニオンの定義からノルムは1である．

2-5

クォータニオン表現による絶対座標系から移動座標系への回転行列Rは，以下のよう に表される．

2-6

n

θ'

2.1.4

クォータニオンの微分方程式

クォータニオンの時間微分は，角速度ベクトル を用いて以下のよう に表される．

2-7

2.1.5

クォータニオからオイラー角への変換式

式2-3よりクォータニオンからオイラー角の変換式は，式2-8^となる．

2-8

2.1.6

クォータニオンによる姿勢角算出の流れ

クォータニオンによる姿勢角算出のフローチャートを図2-2に示す．ジャイロセンサ から得た角速度データ は，ジャイロセンサのバイアスにより微小に値を含んでいる ため，しきい値を設定した．角速度0.01 [rad/s]以下を静止状態ととみなし，0 [rad/s]と ている．初期クォータニオン は，初期角度は， = 0であることから， を 与えた．クォータニオンの微分値を算出し，積分，正規化後， となっ た場合は， にし，次の時間のクォータニオンを算出する．

図2-2 フローチャート（クォータニオンによる姿勢角算出）

クォータニオン積分 正規化

終了

クォータニオン微分値

回転行列R_n の算出 オイラー角に変換

開始

角速度データω_n取得

｜ωn｜< 0.01 rad/s ω_n= [0 0 0]

q_n= [1 0 0 0] q_n= [0 0 0 0]

Yes Yes

No

No

2.2

姿勢計測

慣性センサによる姿勢計測実験を報告する．慣性センサの軸方向は図2-3である．

図2-3 慣性センサの各軸

2.2.1

実験Ⅰ クォータニオンによる姿勢角計算

オイラー角による姿勢演算では，ピッチ角に±90°に特異点が存在する．実験Ⅰでは，

クォータニオンによる姿勢表現からオイラー角の姿勢表現に変換したときの特異点の 影響を検証した．図2-4のように慣性センサは回転する円盤上にある板上にあり，円盤

は360°回転することができる．円盤を1回転させ初期姿勢からの慣性センサが姿勢を

表現できるか確認した．最初に円盤はX軸まわりに回転し，ロール角，ピッチ角，ヨ ー角を算出した．次に，慣性センサの設置向きを変えることで，回転する軸をY軸ま わり，Z軸まわりにし，同様にロール角，ピッチ角，ヨー角を算出した．

図 2-4 実験の概略図

0.2m

rotation center

sensor board

Rotaion direction

2.2.2

実験Ⅱ

実験Ⅱでは，慣性センサの姿勢精度実験をした．慣性センサを水平面上におき，静止 状態から三角定規を用いて，+30°，+ 45°，+60°，0°の順に傾けることにより，初期姿勢 からの姿勢角を計測した．ロール・ピッチ角の測定は平面上に垂直に立つ三角定規上に 慣性センサを数秒停止させ，ヨー角の測定は，紙面上に0°から60°までの回転角を作図 し計測した．実験は手動で慣性センサを動かし，各軸まわり姿勢計測実験は，それぞれ 3回した．

2.2.3

実験Ⅰ 結果及び考察

図2-5は慣性センサを各軸まわりに回転させたときのロール，ピッチ，ヨー角であり，

図2-6はそのときのクォータニオンの4つのパラメータである．図2-6のクォータニオ ンのパラメータ は，各軸まわりとも回転前の値｛1｝から回転後は，値｛-1｝になっ ている．これは， は ^′ と表されるため，回転軸まわりに初期姿勢0°か

らの360°回転していることを表している．また ， ， はそれぞれロール，ピッチ，

ヨー角の回転に対応している．図2-4の姿勢角において，Y軸まわりのピッチ角は，±

90°までしか表現できておらず，±90°をこえるとロール角，ヨー角の値が発散し，姿 勢角を表現できない結果となった．しかし，図 2-6 を見ると， Y 軸まわりのクォータ ニオンの各パラメータは，発散しておらず，クォータニオンによる姿勢表現には問題な い． Y 軸まわりに対し，X 軸，Z 軸まわりでは，ロール角，ヨー角は±180°まで計測 できている． 次に，ピッチ角の発散の原因を記述する．

ピッチ角 π のときオイラー角の回転行列Rは式2-2より，

となる．オイラー角を式2-3から求めると，

となり，ピッチ角が のときオイラー角では姿勢角の計算はできない． も 同様である．オイラー角から回転角を算出する場合，ピッチ角 のときに姿勢角

を表現できないので，クォータニオンからオイラー角に変換する場合も同様に表現できな い．以上の結果から，クォータニオンからオイラー角へ変換した時の回転角の範囲は，表 2-1となる．

a: X軸まわり b: Y軸まわり c: Z軸まわり 図2-5オイラー角に変換後の姿勢角

a: X軸まわり b: Y軸まわり c: Z軸まわり 図2-6クォータニオン

表2-1 オイラー角変換後の姿勢範囲

-180 -120 -60 0 60 120 180

0 5 10 15

Angle[degree]

Time[s] 0 5 10 15

Time[s] 0 5 10 15 20

Time[s]

Roll Pitch Yaw

0 5 10 15

Time[s] 0 5 10 15 20

Time[s]

q0 q1 q2 q3

-1 -0.5 0 0.5 1

0 5 10 15

Time[s]

ロール角 ピッチ角 ヨー角 測定範囲 -180°≦ Roll ≦ 180° -90° < Pitch < 90° -180°≦Yaw≦ 180°