低精度 MEMS センサと汎用 GPS 受信機の融合による高精度航法システムの研究

東京大学大学院工学系研究科航空宇宙工学専攻

56367 成岡優指導教員: 土屋武司講師

2007 年 2 月 15 日

1 緒言

移動体の制御や監視を行う際、対象物体の位置や速度、姿勢といった基本的な情報を精度よく得ることは必要不可欠である。そのような要請は航空機で培われた航法技術である INS/GPS(Inertial Navigation System / Global Positioning System) 複合航法によって解決できるにも関わらず、それらが積極的に活用されている事例はあまりない。なぜなら既存の INS/GPS 実装は精度が第一に求められる航空宇宙用途を意図しており、汎用的に使用されるにはあまりにも大きく、重く、高価であるからである。

そこで航空宇宙分野以外への積極的な技術移転を目指し、民生品で大量に利用されている低精度な MEMS(Micro-Electro Mechanical System) 慣性センサによって構成された INS、並びに汎用 GPS 受信機を用いて、汎用的利用を促進するような小型、軽量、安価でありかつ高精度な INS/GPS を実現できないか検討を行った。

低精度な慣性センサを利用する為、 INS/GPS を定式化する際に特異点が存在するとモデル誤差が拡大し正しい出力が得られない恐れがある。そのため INS/GPS を定式化するにあたって、特異点がなく数学的に単純なモデルを構築できる Quaternion を活用した。

提案したシステムが実現可能であるか確認するため、また提案したアルゴリズムの性能を検討するため、まずシミュレーションを行った。シミュレーションのセンサモデルは、現実の MEMS 慣性センサを元に作成した。

また実機による評価を、提案したシステムのプロトタイプを作成することで行った。プロトタイプでは可能な限りの精度向上を目指し、温度特性や取付け誤差といった較正作業を行った。性能評価では、航空機の飛行履歴をプロトタイプによって取得し、プロトタイプによる結果と、既存の INS/GPS 機器との比較により評価を行った。

2 INS/GPS を構成する機器

INS/GPS は名前のとおり、 INS と GPS、 2 つの航法システムを統合することによって成り立っている。以下本研究で想定する INS と GPS について記す。 2.1 低精度 MEMS 慣性センサを用いた INS

INS は加速度や角速度をセンサから取得し、運動方程式を計算することによって現在の位置や速度、姿勢を出力する航法システムである。

INS は加速度を計測する加速度計、並びに角速度を計測するジャイロといった慣性センサの設置方式により、ストラップダウン方式とジンバル方式の 2 種類に大別できるが、本研究ではストラップダウン方式を採用した。ストラップダウン方式は演算負荷が大きいかわりに、物理的な機構を必要としないため、装置全体として小型軽量化、低コスト化を期待できる方式であり、本研究の目的に適する。

INS を構成する慣性センサとしては、カーナビなどの民生品用として大量に生産される MEMS センサを採用した。 MEMS センサは、サーボ加速度計や Ring Laser Gyro といった航法用を意図して作られた高精度センサに比べると、遥かに小型、軽量、安価である。しかしながら、数分間の使用でゼロ点が変動することや、個体間で特性のばらつきが大きいなど、このセンサで INS を構成しても実用にならない程低精度であることが知られている。 2.2 汎用 GPS 受信機による GPS

GPS は GPS 衛星から送信された電波を GPS 受信機が受信、処理することによって位置や速度を推定する航法システムである。そのため GPS の精度やコストは GPS 受信機の測位方式に主に依存する。本研究では一周波 (L1 電波) のみを受信し、 C/A コード単独測位を行う GPS 受信機を用いることにした。これは小型・軽量・安価な汎用 GPS 受信機であり、主にカーナビなどに用いられている。

3 Quaternion を利用した INS/GPS アルゴリズム

本章では低精度の MEMS 慣性センサに備えて考案した Quaternion を利用した INS/GPS アルゴリズムについて説明する。 3.1 座標系ならびに表記

以下の説明において複数の座標系ならびに表記が使用される。

まず座標系として図 1 に示す通り 5 座標系を取り扱う。

• i-Frame 地球中心が原点、 Z_{i} が地球回転軸に一致する、右手系直交慣性系座標系

• e-Frame 地球中心が原点、 X_{e} が経度 0^{∘} 方向、 Z_{e} が地球回転軸の右手系直交座標系

• g-Frame 観測対象中心が原点、 N_{g} ≡ X_{g} が北方向、 D_{g} ≡ Z_{g} が重力方向の右手系直交座標系

• n-Frame g-Frame を Z_{g} を軸に Azimuth α rad だけ回転した右手系直交座標系

• b-Frame 観測対象中心が原点、 X_{b} が前方、 Z_{b} が下方の右手系直交座標系

図 1: 座標系

また一般的な記号とし列ベクトルは\underline{u}、 3 次元ベクトルは u⃗、 Quaternion は q̃ または

\{ \begin{matrix}q(スカラー要素) \\ q⃗(ベクトル要素) \end{matrix}\} のように表す。 q̃^{∗} ≡ \{ \begin{matrix}q \\ −q⃗ \end{matrix} \} は

共役 Quaternion である。添字については u_{1}^{2} は 1-Frame における値を 2-Frame で観測した値であり、 u_{1/2}^{3} は 2-Frame に対する 1-Frame の値を 3-Frame で観測した値である。

代表的な記号は以下のとおりである。

r⃗_{e} = 地球中心から観測対象へのベクトル
ṙ_{e}^{n} = 地球に対する観測対象の速度
a^{b} = 観測対象に固定された加速度計出力
ω⃗_{e/i}^{e} = 慣性系に対する地球の回転
ω⃗_{n/e}^{n} = 地球に対する n-Frame の回転
ω⃗_{b/i}^{b} = 観測対象に固定されたジャイロ出力
g^{n} = 地球万有引力
h = 高度
q̃_{e}^{n} = 位置 Quaternion(緯度、経度、 Azimuth 角)
q̃_{n}^{b} = 姿勢 Quaternion(ロール、ピッチ、ヨー)
P = システム誤差共分散行列
Q = プロセス誤差共分散行列
R = 観測誤差共分散行列
K = カルマンゲイン

3.2 Kalman Filter による INS と GPS の統合

INS と GPS、両者の統合には確率論的に最も確からしい値を算出する方法である Kalman Filter を利用した。 Kalman Filter の中でも数値的に安定な UD 分解 Kalman Filter を利用した。

Kalman Filter を利用した INS と GPS の統合にはいくつか方法が存在するが、その中で Loose coupling を採用した。 Loose coupling は Kalman Filter におけるシステム方程式が INS の慣性航法方程式、観測方程式が GPS 受信機から出力される位置・速度によって構成される方式である。そのため GPS 受信機によって位置や速度解が得られる状況、すなわち GPS が 4 衛星以上可視でなければならないという必要条件が存在する。しかしながら、計算負荷が軽く、アプリケーションに応じて他の補強システムとの組み合わせが容易であるという汎用的な航法システムを構築するには優れた特性を持つため、本研究では Loose coupling を採用した。図 2 にアルゴリズムの全体像を示す。以降、 Kalman Filter を構成するのに必要となるシステム方程式、観測方程式について述べた後、本研究で用いた INS/GPS アルゴリズムについて述べる。

図 2: Loose coupling INS/GPS アルゴリズム

3.3 システム方程式 (INS)

INS における慣性航法方程式の計算にあたっては、地球上の位置ならびに姿勢の表現として、特異点が存在しない Quaternion を用いた。これは特異点が存在すると、特異点近傍でモデル誤差が大きくなり正しい出力が得られなくなることを回避するためである。特に、 MEMS センサを利用した INS は入力のノイズが大きいため、計算中にモデル空間上のどのような経路を辿るかは不明確である。ゆえに本システムの精度を向上する上で、特異点を排除することは有効であると考えた。

ここにおいて INS の運動方程式は以下のようになる。

• 速度方程式

\begin{aligned}\frac{d}{dt}\{ \begin{matrix}0 \\ ṙ_{e}^{n} \end{matrix}\} & = q̃_{b}^{n}^{∗}\{ \begin{matrix}0 \\  & a^{b} \end{matrix}\} q̃_{b}^{n}+ \{ \begin{matrix}0 \\  & g^{n} \end{matrix}\} \\ & − \{ \begin{matrix}0 \\ ( 2ω⃗_{e/i}^{n} + ω⃗_{n/e}^{n}) ×ṙ_{e}^{n} \end{matrix} \} \\ & −q̃_{e}^{n}^{∗}\{ \begin{matrix}0 \\ ω⃗_{e/i}^{e} × ( ω⃗_{e/i}^{e} ×r⃗_{e}) \end{matrix} \} q̃_{e}^{n} \end{aligned} (1)

• 位置方程式

\frac{d}{dt}q̃_{e}^{n} = \frac{1}{2}q̃_{e}^{n}\{ 0 ; ω⃗_{n/e}^{n}\} , \frac{d}{dt}h = −(ṙ_{e}^{n})_{Z}  (2)

• 姿勢方程式

\begin{aligned}\frac{d}{dt}q̃_{n}^{b} & = \frac{1}{2} [q̃_{n}^{b}\{ 0 ; ω⃗_{b/i}^{b}\} \\ & −( \{ 0 ; ω⃗_{e/i}^{n} \} + \{ 0 ; ω⃗_{n/e}^{n}\} ) q̃_{n}^{b}] \end{aligned} (3)

以上まとめてシステム方程式は

\frac{d}{dt}\underline{x}= f (\underline{x},\underline{u})  (4)

ただしシステムの状態量\underline{x}は

\underline{x}≡ [ \begin{matrix}ṙ_{e}^{n} \\  & q̃_{e}^{n} \\ h \\  & q̃_{n}^{b} \end{matrix}]  (5)

でありシステムの入力\underline{u} は

\underline{u}≡ [ a^{b} ; ω⃗_{b/i}^{b} ; g^{n} ]  (6)

である。 3.4 観測方程式 (GPS)

観測方程式は GPS が出力する位置・速度情報によって構成され、以下のとおりである。

\underline{z}= [ \begin{matrix}I & 0 & 0 & 0 \\ 0 & I & 0 & 0 \\ 0 & 0 & I & 0 \end{matrix}] \underline{x}+ \underline{v}  (7)

ただし観測量\underline{z} は

\underline{z}≡ [ \begin{matrix}ṙ_{e}^{n} \\  & q̃_{e}^{n} \\ h \end{matrix}]_{GPS}  (8)

であり、\underline{v}は観測誤差、つまり GPS 受信機出力の誤差である。 3.5 INS/GPS アルゴリズム

システム方程式は非線形である。そのため非線形系に Kalman Filter を適用する一手法である Externded Kalman Filter に倣い、システム方程式の状態量並びに入力の推定値ˆ\underline{x}、ˆ\underline{u} に対するそれぞれの真値\underline{x}、\underline{u} の差、すなわち誤差 Δ\underline{x}、 Δ\underline{u} に対してシステム方程式を線形化すると同時に、誤差 Δ\underline{x}に対する観測方程式を利用する方法を用いた。ここでは前者を誤差システム方程式、後者を誤差観測方程式と呼ぶことにする。

誤差システム方程式はシステム方程式の状態量の真値\underline{x}に対して

ṙ_{e}^{n} ← ṙ_{e}^{n}+ Δṙ_{e}^{n}  (9)
q̃_{e}^{n} ← \{ \begin{matrix}1 \\ Δu_{e}^{n} \end{matrix}\} q̃_{e}^{n}, h ← h + Δh  (10)
q̃_{n}^{b} ← \{ \begin{matrix}1 \\ Δu_{n}^{b} \end{matrix}\} q̃_{n}^{b}  (11)
a^{b} ← a^{b}+ Δa^{b}, ω⃗_{b/i}^{b} ← ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b}  (12)
g⃗ ← g⃗+ Δg⃗  (13)

とし、導かれた式から元のシステム方程式を引くことによって得られる。ただし Δ は微小量であることをあらわし、 Δ の積は無視する。

ここで Quaternion の誤差は、通常の EKF で用いられる微小差分との線形和ではなく [8] を参考に作成した。線形和によって誤差モデルを表現した場合、 Quaternion のノルムを ||q̃|| と書くならば

\begin{aligned}||q̃+ Δq̃||^{2} & = (q_{0}+ Δq_{0}) ^{2}+ ||q⃗+ Δq⃗||^{2} \\ & ≈ (q_{0}^{2}+ q⃗^{2}) + 2(q_{0}Δq_{0}+ q⃗· Δq⃗) \\ & = 1 + 2(q_{0}Δq_{0}+ q⃗· Δq⃗) \end{aligned} (14)

となり制約条件 ||q̃+ Δq̃|| = 1 を常には満足できない。そこで ||Δu⃗|| ≈ 0 なる微小ベクトル要素 Δu⃗ を導入し、誤差システム方程式における状態量を Quaternion 積で表現すると

q̃+ Δq̃≡ \{ \begin{matrix}1 \\ Δu⃗ \end{matrix}\} q̃= \{ \begin{matrix}q_{0}− Δu⃗· q⃗ \\ q⃗+ q_{0}Δu⃗+ Δu⃗× q⃗ \end{matrix} \}  (15)

\begin{aligned}||q̃+ Δq̃||^{2} & ≡ (q_{0}− Δu⃗· q⃗)^{2} \\ & + ||q⃗+ q_{0}Δu⃗+ Δu⃗× q⃗||^{2} \\ & ≈ (q_{0}^{2}− 2q_{0}Δu⃗· q⃗) \\ & + (||q⃗||^{2}+ 2q_{0}Δu⃗· q⃗) \\ & = q_{0}^{2}+ ||q⃗||^{2} = 1 \end{aligned}  (16)

となり誤差を含んだ状態でもノルムが 1 であることが保証される。ここで

Δ\underline{x}≡ [ \begin{matrix}Δṙ_{n}^{e} \\ Δu_{e}^{n} \\ Δh \\ Δu_{n}^{b} \end{matrix}] , Δ\underline{u}≡ [ \begin{matrix}Δa^{b} \\ Δω⃗_{b/i}^{b} \\ Δg⃗ \end{matrix} ]  (17)

と書けば、誤差システム方程式は

\frac{d}{dt}Δ\underline{x}= AΔ\underline{x}+ BΔ\underline{u}  (18)

となる。すなわち、システム方程式は ṙ_{e}^{n},q̃_{e}^{n},h,q̃_{n}^{b} の全 12 状態量に対し、誤差システム方程式は Δṙ_{e}^{n},Δu_{e}^{n},Δh,Δu_{n}^{b} の全 10 状態量で表現される。

一方、誤差観測方程式は、状態量の推定値ˆ\underline{x} に対し

\begin{aligned}\underline{z}− H ˆ\underline{x} & = −[ \begin{matrix}I & 0 & 0 & 0 \\ 0 & H_{Δ22} & 0 & 0 \\ 0 & 0 & I & 0 \end{matrix}] [ \begin{matrix}Δṙ_{e}^{n} \\ Δu_{e}^{n} \\ Δh \\ Δu_{n}^{b} \end{matrix}] + \underline{v} \\ & ≡ H_{Δ}Δ\underline{x}+ \underline{v} \end{aligned} (19)

と書ける。ただし

H_{Δ22} ≡ [ \begin{matrix}−q_{1} & −q_{2} & −q_{3} \\ q_{0} & q_{3} & −q_{2} \\ −q_{3} & q_{0} & q_{1} \\ q_{2} & −q_{1} & q_{0} \end{matrix} ]_{ˆq̃_{e}^{n}}  (20)

である。

誤差共分散行列 P = E[Δ\underline{x}Δ\underline{x}^{T}],Q = E[Δ\underline{u}Δ\underline{u}^{T}],R = E[\underline{v}\underline{v}^{T}] を導入すると、 GPS の観測量\underline{z}が得られないときは、 Kalman Filter の Time Update を行う。すなわちタイムステップ Δt に対し

ˆ\underline{x}_{k+1} = ˆ \underline{x}_{k}+ f(ˆ\underline{x}_{k}, ˆ\underline{u}_{k})Δt  (21)

P_{k+1} = (I + A_{k}Δt)P_{k}(I + A_{k}Δt)^{T}+ (B_{k}Δt)Q(B_{k}Δt)^{T} (22)

で時間更新を行う。 GPS の観測量\underline{z}が得られた場合は Kalman Filter の Measurement Update、すなわち

K_{k} ≡ P_{k}H_{Δk}^{T}(H_{Δk}P_{k}H_{Δk}^{T} + R_{k})^{−1}  (23)
P_{k} ← (I − K_{k}H_{Δk})P_{k}  (24)
Δx̂_{k}≡ [ \begin{matrix}Δˆṙ_{n}^{e} \\ Δû_{e}^{n} \\ Δĥ \\ Δû_{n}^{b} \end{matrix}]_{k} = K_{k}(z_{k}− H_{k}ˆ\underline{x})  (25)

ˆṙ_{n}^{e} ← ˆ ṙ_{n}^{e}− Δˆṙ_{n}^{e}  (26)
\begin{aligned}ˆq̃_{e}^{n} & ← \{ \begin{matrix}1 \\ (Δû_{e}^{n})_{k} \end{matrix}\} ^{∗} ˆq̃_{e}^{n}, \\ ĥ & ← ĥ− (Δĥ)_{k} \end{aligned}  (27)
ˆq̃_{n}^{b} ← \{ \begin{matrix}1 \\ (Δû_{n}^{b})_{k} \end{matrix}\} ^{∗} ˆq̃_{n}^{b}  (28)

で修正を行う。 3.6 利用上の補記

初期化については、利用する慣性センサの性能が低いため、地球自転角速度を利用したジャイロコンパシングによる精密な初期化は行えない。従って位置・速度は計測開始時に GPS から得られた値とし、姿勢はロール・ピッチは重力から、ヘディングは初動時の速度ベクトルから取得した。誤差共分散行列 P は適当に大きな値で初期化を行った。

誤差共分散行列 Q については静止状態で計測したセンサのノイズを元に設定し、同 R については GPS 受信機が出力する Dilution of precision(DOP) の値を用いた。

4 シミュレーション

低精度な MEMS 慣性センサを用いて INS/GPS 航法を行うことができるか確認する為、また提案したアルゴリズムの性能を検証する為、シミュレーションを行った。 4.1 慣性センサならびに GPS 受信機モデル

シミュレーションを行うにあたり、 INS で用いる慣性センサのモデルは現実の MEMS 慣性センサを利用してパラメータを決定することにより作成した。作成したセンサモデルは 2 種類でありそのノイズ成分を、モデル 1 をホワイトノイズのみによって構成したモデル、モデル 2 をモデル 1 に加えジャイロのランダムドリフトを 1 次マルコフ過程として考慮したモデルである。モデル 1 にくらべモデル 2 はより現実的なモデルとなっている。

一方、 GPS 受信機のモデルは位置・速度にホワイトノイズが加わっているとした。 4.2 水平定常旋回シミュレーション

シミュレーションとして半径 100m の円を 2 分で 1 周する水平定常旋回を行う軌道を仮定した。結果を示すと、図 3、図 4、図 5 が 1 周分の位置履歴、速度履歴、姿勢履歴を表わしている。

図 3: シミュレーション位置履歴 (1 周)

{図 4: シミュレーション速度履歴 (1 周) \atop 図 5: シミュレーション姿勢履歴 (1 周)}

結果より低精度な MEMS 慣性センサによる INS では 2 分程度の使用でも実用にならないが、 GPS 受信機と組合わせて INS/GPS とすることで実用的な推定を行っていることが見て取れる。より長時間のシミュレーションも行ってみた。 5 周分の位置履歴を図 6 に示す。

図 6: シミュレーション位置履歴 (5 周)

より長時間のモデルでも真値に近い値を得られることが確認できた。これにより低精度な MEMS 慣性センサを用いても汎用 GPS 受信機と組合わせることにより実用的な航法を行うことができることを確認できた。 4.3 Euler 角 INS/GPS アルゴリズムとの比較

本研究で用いたアルゴリズムは Quaternion を利用したものであるが、一般的な INS/GPS アルゴリズムは Euler 角を用いて位置や姿勢を表現するのが一般的である。アルゴリズムの違いによる差を論理的に検証するため、位置の表現に緯度、経度といった Euler 角を用いた INS/GPS アルゴリズムとの比較を行った。

比較の方法は各アルゴリズムで Time Update のみを繰り返し行い、システムの誤差共分散行列 P の伝播状態を比較した。初期状態は真値とし、経度、高度、速度、姿勢角についてはすべて 0 とした。緯度は 0 度、 40 度、 80 度の 3 通りを試した。その後 300 秒間センサモデル 1 で静止状態をシミュレーションし、誤差共分散行列 P のうち姿勢の誤差 Δu_{n}^{b} の第一成分に対する対角要素 P_{Δu_{n}^{b}_{X}} を比較した。同時に 1000 回シミュレーションを行いその分散の真値を得た。結果を表 1 に示す。

表 1: 分散 P_{Δu_{n}^{b}_{X}} の伝播比較
初期緯度 [deg] 0 40 80
真値 [×10^{−5} ]  2.16 2.16 2.17
位置:Quaternion [×10^{−5} ] 2.13 2.13 2.13
位置:Euler [×10^{−5} ]  2.13 25.7 62.7

比較結果から低緯度では両アルゴリズムとも正しく分散を推定できているが、高緯度では Euler 角を用いたアルゴリズムは正しく分散を推定できていない。位置に Euler 角を用いたアルゴリズムでは緯度 90 度に特異点が存在するため、高緯度になるに従って非線形の効果が高まり正しく分散を推定できないことを示している。これより本研究で提案した Quaternion によって特異点を排除したアルゴリズムは、演算過程において大きな入力誤差が加わるおそれがある低精度な MEMS 慣性センサを利用するのに有効であると考えられる。

5 性能評価

シミュレーションに引き続き、提案したシステムの精度を検証するため、プロトタイプを作成し、較正作業を行った後、既存の INS/GPS 装置との比較実験を行った。 5.1 プロトタイプ

作成したプロトタイプについては外観を図 7、構成図を図 8、構成詳細を表 2 にあげる。プロトタイプには想定した機器に従い、 3 軸の MEMS 加速度計、ならびに 3 軸の MEMS ジャイロ、汎用 GPS 受信機を搭載している。簡単のため、プロトタイプではリアルタイムでデータ取得のみを行い、後にオフラインで PC にて解析することによって INS/GPS を構成した。なお作成したプロトタイプは、 PC や構造部材を除いて大きさで 100 cc 以下、重さで 30 g 以下、原価で約 3 万円と汎用的に使用するのに十分小さく軽く経済的であることが確認された。 5.2 較正作業

慣性センサの温度特性ならびに取付け誤差を測定し、それらの結果を元に校正した上で以降の性能評図 7: プロトタイプ外観

図 8: プロトタイプ構成図

表 2: プロトタイプ構成詳細

構成機器説明
加速度計  STMicro LIS3L02AS4 1 個

(3 軸 1 パッケージ, MEMS)

ジャイロ  AnalogDevices ADXRS150 3 個

(1 軸 1 パッケージ, MEMS)

温度センサジャイロに内蔵のもの
GPS  u-blox TIM-LA 1 個

(民生用 L1 電波受信 GPS, 4Hz 出力)

A/D 変換機 AnalogDevices AD7739

(100Hz, 24bit 変換)

データ処理 USB で PC と接続
サイズ  100 cc 以下 (PC、構造部材を除く)
重量  30 g 以下 (PC、構造部材を除く)
価格約 3 万円

価実験を行った。これらの較正作業は慣性センサを利用する上で必須の作業である。調達できた校正用機器の関係から、加速度計ならびにジャイロのゼロ点の温度特性とジャイロの取付け誤差を評価した。以下その詳細について述べる。 5.2.1 ゼロ点の温度特性

ゼロ点の温度特性は恒温槽にプロトタイプを静置し、温度を変化させることで結果を取得した。測定されたデータの一部を図 9 に示すが、これより温度とゼロ点の変動には線形関係があることが見て取れる。測定結果より求められた結果を表 3 に示す。

図 9: X 軸ジャイロゼロ点温度特性

表 3: ゼロ点温度特性

センサ  X 軸  Y 軸  Z 軸
\begin{matrix}加速度計 \\ [m/s^{2}/^{∘}C] \end{matrix} -0.000763 0.0223 0.00602
\begin{matrix}ジャイロ \\ [deg/^{∘}C] \end{matrix}  0.0575 -0.0881 -0.0191

5.2.2 ジャイロの取付け誤差

ジャイロの取付け誤差の測定は、一定速度で回転するレートテーブル上にプロトタイプを固定し計測を行うことにより求められる。例えば x 軸ジャイロの主軸方向の単位ベクトル、スケールファクタをそれぞれ u⃗_{x}、 K_{x}、ある取付け面 ( 1 面のように表記) の法線単位ベクトルを u⃗_{01}、 1 面を基準面としたときの x 軸ジャイロの出力値を a⃗_{x1} のように表記すれば、 1 面、 2 面、 3 面について次の関係が存在する。

[ \begin{matrix}\frac{a⃗_{x1}}{K_{x}} & \frac{a⃗_{y1}}{K_{y}} & \frac{a⃗_{z1}}{K_{z}} \\ \frac{a⃗_{x2}}{K_{x}} & \frac{a⃗_{y2}}{K_{y}} & \frac{a⃗_{z2}}{K_{z}} \\ \frac{a⃗_{x3}}{K_{x}} & \frac{a⃗_{y3}}{K_{y}} & \frac{a⃗_{z3}}{K_{z}} \end{matrix} ] = [ \begin{matrix} & u_{01}^{T} \\  & u_{02}^{T} \\  & u_{03}^{T} \end{matrix}] [u⃗_{x} u⃗_{y} u⃗_{z}] (29)

従って u⃗_{01} 等が既知である (例えば直交する)3 つの面それぞれについて計測を行えば、 a_{x1} 等が計測され、 |u⃗_{x}| = 1 等の条件から K_{x} 等が求められると同時に、取付け誤差をあらわす u⃗_{x} 等が求められる。結果を表 4 にあげる。

表 4: ジャイロ取付け誤差

X 軸  Y 軸  Z 軸
1.00  8.72E^{−3} −1.18E^{−2}
u⃗ −9.43E^{−3} 1.00 4.91E^{−3}
1.52E^{−3} 1.24E^{−2}  1.00
{取付け誤差 \atop [deg]}  0.547  0.868  0.731

5.3 性能評価実験

作成したプロトタイプと既存の INS/GPS 機器との比較により提案したシステムの精度を確認した。ここではその中でも高精度 INS/GPS 航法装置である GAIA[12] との比較について記す。 GAIA は絶対位置誤差で 1m と、リファレンスとして十分に信頼できる精度を有する装置である。実験に際しては JAXA 所有の実験用航空機 MuPAL-α を用い、プロトタイプと GAIA を搭載した状態で水平定常飛行や旋回定常飛行等を行った。プロトタイプと GAIA の出力値の比較を図 10 に位置履歴を、図 11 に速度履歴を、図 12 に姿勢履歴を示す。

それぞれの履歴からプロトタイプと GAIA の出力はほぼ一致していることが見て取れる。そこで定量的に評価した結果を表 5 にあげる。なお、これらの値は KF の誤差共分散 P が十分に収束したと思われる時刻 (GPS 時刻で 3.58 × 10^{8} msec) 以降の値である。またプロトタイプと GAIA の取付け位置による差は考慮してあるが、取付け角度差は考慮していない。図 10: 位置履歴 図 11: 速度履歴 図 12: 姿勢履歴表 5: プロトタイプと GAIA の差異

平均値標準偏差最悪値
水平距離 [m]  5.39  2.17  10.6
高度 [m]  8.80  4.49  18.1
北方向速度 [m/s] 0.00  0.13  1.44
東方向速度 [m/s] -0.01  0.13  -1.29
下方向速度 [m/s] -0.32  0.17  -0.93
ロール [deg]  -0.01  0.23  -0.91
ピッチ [deg]  0.11  1.01  3.22
ヘディング [deg] 1.61  6.96  19.0

GAIA の値を真値として以下の議論を進めると位置や速度といった GPS 受信機から直接の補強が得られる状態量については、多少のオフセットが発生してしまっているものの、非常によく推定できている。まず位置についてであるが、最悪値を見ても水平方向でほぼ 10m 以内を実現できている。平均値と標準偏差を見れば本システムで当初目標性能とした位置で数 m 以内という性能を達成できていることがわかる。高度方向については多少精度が劣化し 20m 程度であるが、これは GPS が性能的に水平方向よりも高度方向に精度が劣化していることを考えれば妥当な値である。

速度についてもプロトタイプと GAIA でほぼ差異がない。 GPS 受信機による直接的な補強が非常に寄与しているものだと考えられる。

姿勢についていえば、ロール、ピッチでは非常によく推定ができているといえる。先述したとおり、プロトタイプと GAIA の取付け傾きが測定できていないことから標準偏差のみを利用して議論を行うと、ロール、ピッチの精度は約 1 度程度であると考えられる。一方ヘディングであるが、こちらは標準偏差で 7deg も誤差がある。この差異を詳しく見るため、ヘディングを速度方向との差、すなわち対地速度に対する横滑りで見ることにした。図 13 に対地速度に対するプロトタイプと GAIA の横滑りの比較を示す。

これよりプロトタイプによるヘディングは、ドリフトをおこすほど精度が劣化していることが読み取

図 13: 対地速度に対する横滑り履歴

れる。

ヘディングのみ精度が悪くなる理由として、ロールやピッチは対象物体が加速によって受ける加速度よりもはるかに大きい加速度である重力を頼りにできることが関連していると考えられる。言い換えれば、加速環境下においても、その状態が水平定常旋回運動のように定常的なものでない限りは、重力を参考にすることによってロールやピッチも精度は粗いながら求めることは可能である。しかしながら、ヘディングは地球の自転速度以外に特にリファレンスはない。言い換えれば、たとえ静止状態であっても精度が悪いジャイロでは地球の自転角速度を捕らえることができない。これよりヘディングの悪さはプロトタイプと GAIA とで利用している慣性センサ、特にヘディングは姿勢であるからジャイロの違いが最も現れていると考えられる。

このジャイロによる差を明らかにするため、プロトタイプのジャイロを GAIA のジャイロに置き換えてみた。表 6 に統計的にまとめた値を、図 14 に対地速度に対する横滑り角を示す。明らかにヘディングを含め全体の精度が改善している。これより特に MEMS ジャイロの精度が低いと結論付けられる。

また、より精密に誤差要因をモデル化できれば提案したシステムを高精度化できる可能性があると考え、加速度計とジャイロ、両慣性センサのバイアス (ゼロ点) 変動推定を組み込んだアルゴリズムを試してみた。このアルゴリズムは誤差システム方程式の表 6: プロトタイプと GAIA の差異 (GAIA ジャイロを利用)

平均値標準偏差最悪値
水平距離 [m]  5.12  2.08  11.1
高度 [m]  7.86  2.18  11.7
北方向速度 [m/s] 0.00  0.10  1.46
東方向速度 [m/s] 0.00  0.10  -1.20
下方向速度 [m/s] -0.33  0.04  -0.43
ロール [deg]  -0.01  0.11  -1.19
ピッチ [deg]  -0.00  0.086  -0.57
ヘディング [deg] -0.20  0.95  -5.02

図 14: GAIA との比較実験 (GAIA ジャイロを利用) 対地速度に対する横滑り履歴

状態量 Δ\underline{x}を拡大し、加速度計ならびにジャイロのバイアス Δa^{b}_{bias}、 Δω⃗_{b/i}^{b} _{bias} を組み込み、その変動を 1 次マルコフ過程でこれらを推定するものである。この誤差システム方程式は全 16 状態量となっている。結果を表 7 に統計的にまとめた値を、図 15 に対地速度に対する横滑り角を示す。

結果は、姿勢についてみるとロール、ピッチ、ヨーのどれでも精度が多少改善されていることがわかる。特にヘディングについては最悪値で 6.96deg から 5.75deg と 1deg へと改善している。しかしながら対地速度に対する横滑りの誤差の傾向はいまだにバイアス変動推定がないものと同じである。このことから考えるに今回利用した MEMS 慣性センサ、特にジャイロは、バイアス変動以外のモデル化できていない誤差要因がある、あるいはモデル化されて表 7: プロトタイプと GAIA の差異

(バイアス変動推定付き)

平均値標準偏差最悪値
水平距離 [m]  5.72  2.27  11.9
高度 [m]  7.23  3.46  14.4
北方向速度 [m/s] 0.00  0.13  1.44
東方向速度 [m/s] -0.01  0.13  -1.26
下方向速度 [m/s] -0.36  0.16  -0.87
ロール [deg]  -0.04  0.20  -0.87
ピッチ [deg]  -0.00  0.84  2.36
ヘディング [deg] 0.95  5.75  15.3

{図 15: 対地速度に対する横滑り履歴 \atop (バイアス変動推定付き)}

いる部分についてもパラメータ自体が時間変化しているなど、不確定な要因が存在しセンサの精度を劣化させていると考えられる。

6 結言

本研究のまとめとして以下のことをあげる。本研究で提案した INS/GPS が実現可能であるこ

とをシミュレーションにより確認できた。また提案したアルゴリズムは、特異点を排し高い精度で分散が推定できる優れたアルゴリズムであることをシミュレーションは示した。

シミュレーションに加え実機による比較実験から、本研究で提案したシステムは位置精度で水平誤差 10m 以内、垂直誤差で 15m 以内、姿勢はロール誤差、ピッチ誤差で 1deg 以内、ヘディング誤差で 10deg 以内であった。これは野外での使用を仮定するなら十分に汎用的に利用可能な精度だと考えられる。捕捉になるが、これは中程度の精度をもつ比較的安価 (百万円程度) な市販の INS/GPS 機器、 Crossbow 社製 NAV420 と同等の性能であることが別の比較実験より得られている。

精度を劣化させている主な原因は MEMS ジャイロにあることが得られた。この精度を大きく改善するためには、 2 つのアプローチがあると考えられる。

1 つはハードウェア的なアプローチである。単純な方法であるがジャイロをより精度の高いものに変更すればより高い精度を実現できる。しかしながら本システムはジャイロをできるだけ小型軽量安価な汎用的な製品を使うことによって実現されるものであるから、より精度の高いジャイロが汎用的に使用できるような技術革新の到来が待たれる。あるいはジャイロを補強するようなシステム、特に地磁気センサや GPS コンパスなどと組み合わせる方法が有効であろう。

もう 1 つはソフトウェア的なアプローチである。精度が悪いジャイロをより詳しく分析し、より詳細な数学モデルを作成することによって精度を改善できる可能性がある。他の先行研究を参照すると、航空宇宙用ないしはそれに順ずる航法装置の高精度化を、ニューラルネットワークやウェーブレットフィルタなどを用いて実現している。このような手法は今後検討に値するであろう。

最後に、本研究で提案した INS/GPS システムは汎用的に使用するのに十分なサイズ、重量、経済性を達成しながらも、低精度な MEMS 慣性センサから見ればはるかに高精度な航法システムを実現することができた。このことは航空宇宙用の航法技術が他分野で活用される大きな契機となることを期待したい。なお現在、リアルタイム演算が可能であり小型 (51 × 51 × 48mm)、軽量 (84.0g) な新プロトタイプ (図 16) を開発中である。

参考文献

[1] Robert M. Rogers, Applied Mathematics in Integrated Navigation System, Second Edition.

図 16: 新プロトタイプ

AIAA Education Series, 2003. ISBN 1-56347-656-8.

[2] Dan Simon, Optimal State Estination. Wiley-Interscience, 2006. ISBN 978-0-471-70858-2.

[3] Factorization Methods for Discrete Sequential Estimation. Academic Press, Orlando, FL, 1977.

[4] S. Winkler, M. Buschmann, L. Kruger, H. Schulz and P. Vörsmann, “State Estimation by Multi-Sensor Fusion for Autonoumus Mini and Micro Aerial Vehicles”, Paper 2005-5840 at AIAA Guidance, Navigation, and Control Conference and Exhibit, 2005.

[5] J. Liu, R. Li, X. Niu, and L. Qiao, “MEMS-Based Inertial Integrated Navigation Technology for Micro Air Vehicles ”, Papaer 2006-6547 at AIAA Guidance, Navigation, and Control Conference and Exhibit, 2006.

[6] Walid Abdel-Hamid, “Accuracy Enhancement of Integrated MEMS-IMU/GPS Systems for Land Vehicular Navigation Applications”, Ph.D thesis, University of Calgary 2005.

[7] R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems”, Transactions of the ASME–Journal of Basic Engineering, Vol.82, D, pp.35-45, 1960.

[8] A. Ude, Filtering in a unit quaternion space for model-based object tracking. Robotics and Autonomous Systems, vol.28, pp.163-172, 1999.

[9] J. Wendel, J. Metzger, R. Moenikes, A. Maier, G. F. Trommer, A Performance Comparison of Tightly Coupled GPS/INS Navigation Systems based on Extended and Sigma Point Kalman Filters. Navigation, vol.53, No.1, pp.21-31, 2006.

[10] John L. Crassidis, Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation IEEE Transactions on Aerospace and Electronic Systems, vol.42, Issue.2, pp.750-756, 2006.

[11] Chiang Kai-Wei, Noureldin Aboelmagd, El-Sheimy Naser, A New weight updating method for INS/GPS integration architectures based on Neural Network Measurement Science and Technology, Volume 15, Issue 10, pp. 2053-2061, 2004.

[12] 張替正敏, 冨田博史, 西澤剛志, 高精度 GPS 補強型慣性航法システムの開発. 日本航空宇宙学会論文集, Vol.50, No.585, pp.416-425 2002.

[13] D.Choukroun, “A Novel Quaternion Kalman Filter”, Paper 2002-4460 at 42th AIAA Guidance, Navigation, and Control Conference, 2004.

[14] D. W. Allan, Time and frequency (time domain) characterization, estimation and prediction of precision clocks and oscillators IEEE trans. UFFC, vol UFFC - 34, No.6, Nov. 1987.

[15] 多摩川精機�『ジャイロ活用技術入門』工業調査会, 2003. ISBN 4-7693-1208-3.

[16] 遠坂俊昭『計測のためのフィルタ回路設計』 CQ 出版社, 1998. ISBN 4-7898-3282-1.

[17] Tsui, James Bao-yen. Fundamentals of global positioning system recievers : a software approach - 2nd ed.. Wiley-Interscience, 2005. ISBN 0-471-70647-7.

特記事項

学会発表

• 2006 年 4 月航空宇宙学会第 37 期年会講演会、 “MEMS センサを用いた INS/GPS 複合航法システム”、成岡優、土屋武司

• 2006 年 9 月日本機械学会 2006 年度年次大会、 “汎用センサを用いた低コスト INS/GPS 複合航法システム”、成岡優、土屋武司

• 2006 年 9 月日本機械学会 2006 年度年次大会、 “ビデオデータを用いたパイロット着陸操縦のニューラルネットワーク分析”、森 亮太、成岡 優、土屋武司、鈴木真二

• 2006 年 11 月航空宇宙学会第 44 回飛行機シンポジウム、 “MEMS センサを用いた小型 INS/GPS 航法装置の開発”、成岡優、土屋武司

• 2006 年 11 月 KSAS-JSASS Joint International Symposium on Aerospace Engineering、 “A Portable and Cost-effective Configuration of Strap-down INS/GPS for General-purpose Use”、成岡優、土屋武司

• 2006 年 11 月 COE シンポジウム第 3 回革新的飛翔物体に関する国際会議、 “A Navigation Avionics for Small UAVs”、成岡優、土屋武司

• 2006 年 11 月 COE シンポジウム第 3 回革新的飛翔物体に関する国際会議、 “The Avionics and Ground Station for Autonoumous Indoor Flyer”, 成岡優、他 7 名

• 2007 年 4 月航空宇宙学会第 38 期年会講演会、 “”、成岡優、土屋武司

特許

• 出願日: 2006 年 4 月 3 日、特願 2006-101456(申請中)、 “移動体制御装置及び移動体制御方法” 、成岡優、土屋武司

その他

• 2003 年情報処理推進機構 (IPA) 未踏ソフトウェア創造事業 (H15 年未踏ユース) “XML を利用した教育コンテンツフレームワークの開発とその利用”、石川啓、葛上昌司、成岡優、田

島知幸、吉田憲吾、千保理

• 2006 年 11 月より Low-cost INS/GPS システムの開発についてセイコーエプソンと共同研究中