MEMS センサを用いた小型 INS/GPS 航法装置の開発

Development of a Compact INS/GPS using MEMS sensors

{56367 成岡優 \atop 指導教員: 土屋武司助教授}

Abstract In this paper, we develop a new compact INS/GPS integrated navigation system. It is aimed for general-purpose use, and by using MEMS sensors and quaternion based modeling it overcomes a problem that existing INS/GPS devices are big, heavy and expensive. In order to examine the effectiveness of our system, we build a prototype instrument based on the suggested design and do an experiment for comparing it with GAIA, a high-precision INS/GPS device developed by Japan Aerospace Exploration Agency (JAXA). The result shows our system is small, light and low-cost enough for general-purpose use and has precision enough for controlling or monitoring general moving objects.

1 緒言

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

そこで著者は航空機以外への積極的な技術移転を目指し、民生品で大量に利用されている MEMS(Micro-Electro Mechanical System) 部品を用いて、新たに小型、軽量、安価な INS/GPS の開発を行った。また、このような構成では入力のノイズが大きいため、 INS/GPS を定式化する際に特異点が存在するとモデル誤差が拡大し正しい出力が得られない恐れがある。そのため INS/GPS を定式化するにあたって、特異点がなく数学的に単純なモデルを構築できる Quaternion を活用した。

提案した INS/GPS の構成が有効であるか確認するため、プロトタイプを作成し性能評価を行った。プロトタイプの作成ではできる限りの精度向上を目指し、温度特性や取付け誤差の較正作業を行った。また性能評価実験では、宇宙航空開発研究機構 (JAXA) 所有の高精度 GPS 補強型慣性航法装置 GAIA(GPS Aided Inertial navigation Avionics [1]) との比較実験を行った。この比較実験では、同機構所有の実験用航空機 MuPAL-α にプロトタイプと GAIA を搭載し飛た上で飛行を行い、両者の出力を定量的に評価した。

2 INS/GPS の構成

本章では本研究の対象となる INS、 GPS、また両者を組み合わせた INS/GPS の詳細について説明する。 2.1 座標系ならびに表記

以下の説明において複数の座標系ならびに表記が使用される。本節ではそれについて定義を行う。

本研究では図 1 に示す通り 5 座標系を取り扱う。

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

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

図 1 座標系

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

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

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

次に記号について定義を行う。 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⃗ 位置

• ω⃗ 角速度

• g⃗ 重力

• q̃_{e}^{n} 地球上における緯度経度の別表現

• q̃_{n}^{b} 観測対象の姿勢

を用いる。 2.2 INS

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

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

また、 INS の精度やコストはどのようなセンサを用いるかに依存する。本研究ではカーナビなどの民生品用として大量に生産される MEMS センサを採用した。 MEMS センサは、サーボ加速度計や Ring Laser Gyro といった航法用を意図して作られた高精度センサに比べると、遥かに小型、軽量、安価である。しかしながら、数分間の使用でゼロ点が変動することや、個体間で特性のばらつきが大きいなど、このセンサで構成された INS 単体の性能は非常に低いことが知られている。従って本研究では、もう一つの航法システムである GPS と組み合わせることによって、その欠点が補償されることを期待する。

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)

2.3 GPS

GPS は GPS 衛星から送信された電波を受信することによって位置や速度を推定する。そのため GPS の精度やコストは GPS 衛星より送信された電波を受信する受信機やアンテナに依存する。より具体的に述べれば、受信周波数帯域や受信機のアルゴリズムなどに依存する。本研究では L1 電波 (1575MHz) を受信し、 C/A コード測位を行う GPS 受信機を用いることにした。これは一般的なカーナビなどに用いられている小型・軽量・安価な GPS 受信機と同等のものである。 2.4 INS/GPS

本研究では、 INS と GPS の統合にあたっては多くの INS/GPS で用いられているカルマンフィルタ (KF) を用い、 INS から得られる位置・速度・姿勢出力に対して、 GPS から得られる位置・速度出力によって補正をかける Loose coupling 方式を採用した。本研究で採用した INS/GPS アルゴリズムを図 2 に示す。

図 2 INS/GPS アルゴリズム

INS の運動方程式は非線形であるため、拡張カルマンフィルタ (EKF) を適用する。そのために INS の真値に対する誤差モデルを作成することになるが、これは

ṙ_{e}^{n} → ṙ_{e}^{n}+ Δṙ_{e}^{n}  (4)
q̃_{e}^{n} → \{ \begin{matrix}1 \\ Δu_{e}^{n} \end{matrix}\} q̃_{e}^{n}, h → h + Δh  (5)
q̃_{n}^{b} → \{ \begin{matrix}1 \\ Δu_{n}^{b} \end{matrix}\} q̃_{n}^{b}  (6)
a^{b} → a^{b}+ Δa^{b}, ω⃗_{b/i}^{b} → ω⃗_{b/i}^{b} + Δω⃗_{b/i}^{b}  (7)
g⃗ → g⃗+ Δg⃗  (8)

とすることにより得られる。ただし Δ は真値との微小差分を表す。

ここで Quaternion の誤差モデルは、通常の EKF で用いられる微小差分との線形和ではなく [2] を参考に作成した。線形和によって誤差モデルを表現した場合、 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}  (9)

となり制約条件 ||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} \}  (10)

\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}  (11)

となり誤差を含んだ状態でもノルムが 1 であることが保証される。よって微小ベクトル要素 u⃗ を用いて EKF に適用する誤差モデルを構築した。すなわち、システムは ṙ_{e}^{n},q̃_{e}^{n},h,q̃_{n}^{b} の全 12 状態量に対し、誤差モデルは Δṙ_{e}^{n},Δu_{e}^{n},Δh,Δu_{n}^{b} の全 10 状態量で表現される。

ここで

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

と書けば、誤差モデルの運動方程式は

\frac{d}{dt}x = Ax + Bu  (13)

と書ける。また GPS の状態量と INS の状態量の差を

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

と書けば、 GPS の観測誤差が v とすると

z = Hx + v  (15)

と書ける。

従って、誤差共分散行列 P = E[xx^{T}],Q = E[uu^{T}],R = E[vv^{T}] を導入すると、 INS の観測量が得られないときは、 KF の Predict の式を用い

P_{k+1} = (I + AΔt)P_{k}(I + AΔt)^{T}+ (BΔt)Q(BΔt)^{T}  (16)

で時間更新を行う。 GPS の観測量が得られた場合は、 KF の Correct の式を用い

K_{k} = P_{k}H_{k}^{T}(H_{k}P_{k}H_{k}^{T}+ R_{k})^{−1}  (17)
P_{k} → (I − K_{k}H_{k})P_{k}  (18)
x_{k} ≡ [ \begin{matrix}Δṙ_{n}^{e} \\ Δu_{e}^{n} \\ Δh \\ Δu_{n}^{b} \end{matrix}]_{k} = K_{k}z_{k}  (19)
(ṙ_{n}^{e})_{INS} → (ṙ_{n}^{e})_{INS}− (Δṙ_{n}^{e})_{k}  (20)
(q̃_{e}^{n})_{INS} → \{ \begin{matrix}1 \\ (Δu_{e}^{n})_{k} \end{matrix}\} ^{∗}(q̃_{e}^{n})_{INS},
h_{INS}→ h_{INS}− (Δh)_{k}

(21)

(q̃_{n}^{b})_{INS} → \{ \begin{matrix}1 \\ (Δu_{n}^{b})_{k} \end{matrix}\} ^{∗}(q̃_{n}^{b})_{INS}  (22)

で修正を行う。

3 性能評価

提案したシステムを検証するため、前章であげた構成にならってプロトタイプを作製し、較正を行った後、 JAXA 所有の高精度 INS/GPS 装置である GAIA との比較実験を行った。 3.1 プロトタイプ

プロトタイプについては、外観を図 3、構成図を図 4、構成詳細を表 1 にあげる。簡単のため、プロトタイプではリアルタイムでデータ取得のみを行い、後にオフラインで PC にて解析することによって INS/GPS を構成した。作成したプロトタイプから考えるに、提案したシステムは汎用的に使用するのに十分小さく軽く経済的であると考えられる。

なおこのプロトタイプを使用して INS/GPS を構成するにあたり、システムの初期化は次のよう行った。位置・速度は計測開始時に GPS から得られた値とし、姿勢はロール・ピッチ・ヨーとも 0 deg とし、 KF の誤差共分散行列 P は適当に大きな値を用いた。また、 KF の誤差共分散行列 Q については静止状態で計測したセンサのノイズを元に設定し、同 R については GPS が出力する Dilution of precision(DOP) の値を用いた。

図 3 プロトタイプ外観

図 4 プロトタイプ構成図

表 1 プロトタイプ構成詳細

構成機器説明
加速度計  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 以下 (構造部材を除く)
重量  30 g 以下 (構造部材を除く)
価格約 3 万円

3.2 性能評価実験

作成したプロトタイプと高精度 INS/GPS 航法装置である GAIA との比較を行った。 GAIA はリファレンスとして十分に信頼できる精度を有する装置である。実験に際しては JAXA 所有の実験用航空機 MuPAL-α を用い、プロトタイプと GAIA を搭載した状態で水平定常飛行や旋回定常飛行等を行った。プロトタイプと GAIA の出力値の比較を図 5、図 6、図 7 に示す。なお、以降の結果はプロトタイプと GAIA の取付け位置の差を考慮し補正をしてある。

また定量的に評価した結果を表 2 にあげる。なお、これらの値は KF の誤差共分散 P が十分に収束した時刻 (GPS 時刻で

図 5 位置履歴

図 6 速度履歴

図 7 姿勢履歴

3.57E^{+8} msec) 以降の値である。

GPS の補正が直接的に得られる位置や速度については GAIA とほぼ一致することが確認された。また GPS の補正が直接は得られない姿勢についても、 GAIA と比較してロールとピッチについては差が数度と非常によい結果が得られた。一方、ヘディングについては十数度の差がある。 3.3 考察

標準偏差を考慮すると、実験結果から得られた本システムの誤差は、位置にして数 m 程度、姿勢についても最悪のヘディングでも十数度と、移動体の制御や監視等に本システムは十分な性能を有すると言える。このことはまた、単体では実用性が乏しいと言われてる MEMS センサを用いた INS でも、本システムのように

表 2 プロトタイプと GAIA の差異

平均値標準偏差最悪値
水平距離 [m]  6.44  2.97  17.0
高度 [m]  0.85  2.10  6.90
北方向速度 [m/s] 0.00  0.12  1.25
東方向速度 [m/s] 0.00  0.12  -1.13
重力方向速度 [m/s] -0.08  0.10  -0.67
ロール [deg]  0.00  0.26  -1.19
ピッチ [deg]  -0.67  1.21  -3.90
ヘディング [deg]  4.17  9.68  23.9

GPS と組み合わせることによって全体として有効なシステムを構築できることを示している。

個々の結果について詳しく見ると、直接 GPS の観測量が得られない姿勢については、ロール・ピッチに比べヘディングの結果がよくないことが特徴的である。これは実験対象として用いた航空機の運動形態を反映しているものと思われる。すなわち前者は数 Hz 以上と比較的高い周波数の運動であるのに対し、後者は周期にして数秒以上の低い周波数の運動である。そして、除去が難しいノイズ成分であるセンサのゼロ点変動なども周期にして数秒以上の低い周波数の現象であるため、ノイズと運動を分離できず、後者の精度が低くなったと考えられる。さらに詳しく実験結果をみると、ピッチについてもそのような現象が観測される。 GPS 時刻 3.58E^{+8} 付近で、プロトタイプと GAIA のピッチがしばらく逆転しているが、この時刻はまさに定常旋回運動を行っている時刻であり、姿勢運動の周波数が低くなっている。

このようなことから判断するに、本研究で提案したシステムをさらに高精度化するためには、対象の運動モードを適切に把握し、特に周波数の低い運動とノイズ成分を分離することが肝心と思われる。具体的には周波数領域で適切なフィルタリングを行うことで精度が向上するであろう。また精度向上のために GPS 以外の補強を考える場合においても、変位の周波数が低い状態量を補強するシステムが有効であると考えられる。例えば本研究で実験対象として選んだ航空機の場合であれば、ヨー方向の補強システムとして磁気コンパスを利用するのがよいと思われる。

4 結言

本研究で提案した INS/GPS システムは汎用的に使用するのに十分なサイズ、重量、経済性を達成できることを作成したプロトタイプは示した。また実験により、提案したシステムの精度は移動体の制御や監視に十分なものであることが得られた。以上により、本システムは汎用的に使用できる INS/GPS だと言える。

加えて、本システムの更なる精度向上の為の新たな方向性が得られた。実験結果によれば、対象の運動周波数と精度が大きく相関していることが考えられる。実験データの周波数領域での解析を進めることが、今後は必要であろう。

参考文献

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

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