MEMS センサを用いた低コスト INS/GPS 複合航法システム

〇成岡優 (東大院), 土屋武司 (東大工)

A Low-cost Integrated Navigation System of INS/GPS Using MEMS Sensors Masaru Naruoka and Takeshi Tsuchiya (University of Tokyo)

Key Words : INS/GPS, MEMS, Kalman Filter, Quaterion

Abstract

An INS/GPS integrated navigation system has good precision and great potential for widespread use, though it has not been used as a low cost solution because of its expensive components. In this research, a new INS/GPS is developed with MEMS sensors and a GPS which make up a consumer car navigation system, and It is far more inexpensive than exsisting ones. A flight test shows it gives about the same good precision as an INS/GPS product.

1 研究背景

現代において位置や速度、姿勢などの状態量を精度よく知りたいという要請は至るところにある。例えば航空機のナビゲーションや電車・自動車などの移動体のモニタリング、ロボットの制御等である。

その要請に応える手段の一つに、 INS/GPS という航法システムがある。これは加速度や角速度をセンサで検知しそれを積算することによって現在の位置や速度、姿勢を推定する Inertial Navigation System(INS) と、地球周回衛星から受信した電波を処理することによって位置や速度を推定する Global Positioning System(GPS) を組み合わせたシステムである。 INS は自己完結的なシステムであり高い更新周期を実現できるが、その精度は使用時間とともに低下する。一方 GPS は高い精度をもつが更新周期は低く、また衛星を補足できる環境にある必要がある。そこで両者を統合し、できうるかぎりの自己完結性と高い更新周期・精度を両立させようとしたのが INS/GPS である。

だが、既存の INS/GPS は性能を最優先したため、特注部品を用いて構成されることが多く、その価格は安価なものでも数百万円である。精度を多少犠牲にしても INS/GPS が従来よりも遥かに低価格で実現できれば、先述の事例以外にも多くの分野でこのシステムが活用されるであろう。そこで本研究では低価格な部品で INS/GPS を構成し、その精度について検証した。 2 INS/GPS の構成

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

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

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

図 1 座標系

• 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} が機軸方向の右手系直交座標系

次に記号について定義を行う。 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 は加速度や角速度などの慣性力を観測するセンサの設置方式により 2 種類に大別できるが、本研究では物理的な機構を必要とせず、観測対象にセンサを直接固定するストラップダウン方式を採用した。

また、 INS の性能ならびにコストの大半はセンサに依存する。本研究では民生品用として大量に生産される安価 (1 個数千円) であるが、バイアス変動が大きい MEMS タイプのセンサを用いることにした。 INS 単体では数万円程度での構成を予定した。

具体的な構成を図 2 にあげる。ここで a^{b} ≡ a_{i}^{b} が加速度計、 ω_{b/i}^{b} がジャイロの出力である。

図 2 INS 構成

また運動方程式の計算にあたっては特異点の問題が発生しない Quaternion を用いた。運動方程式は以下のようになる。

• 速度運動方程式

\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} \} \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 の精度やコストは受信する電波帯域などに依存する。本研究ではカーナビ用の 1 万円程度の安価な L1 電波 (1575MHz) 用受信 GPS を用いることにした。 2.4 INS/GPS

本研究では、 INS と GPS の統合にあたっては多くの INS/GPS で用いられている Kalman Filter を用い、 INS からえられる位置・速度・姿勢情報を GPS から得られる位置・速度情報によって補正をかける Loose coupling 方式を採用した。

INS の運動方程式は非線形であるため、拡張 Kalman Filter を適用する。そのために 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 の誤差モデルは、精度向上のため誤差との線形和ではなく [4] を参考に作成した。システムは ṙ_{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} ]  (9)

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

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

と書ける。また 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}  (11)

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

z = Hx + v  (12)

と書ける。

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

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

で更新を行えばよい。 GPS の観測量が得られた場合は、 Kalman Filter の Correct の式を用い

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

(18)

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

で更新を行えばよい。 3 実験

実機においてその精度を検証するため、前章であげた構成にならって機材を作製し、航空機に搭載した。 3.1 実験機材

実験機材については、外観を図 3、構成図を図 4、構成詳細を表 1 にあげる。簡単のため、今回はリアルタイムでデータ取得のみを行い、後に PC にて解析することによって INS/GPS を構成した。各センサは、スケールファクタエラーやミスアライメントなどの誤差

図 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 と接続
価格約 3 万円

要因をキャリブレーションで補正せずに、カタログスペックを特性値として採用した。

また性能を比較するために、高性能ながら安価な部類 (定価百数十万円) に属する市販の INS/GPS、 Crossbow 社製 NAV420 を同時に搭載した。実験機材の解析における初期値は NAV420 と一致するようにした。 3.2 結果

試験装置よりデータを取得し、そのデータを前述の INS/GPS アルゴリズムによって解析を行った。解析結果と NAV420 との比較を図 5、図 6、図 7 に示す。

図 5 位置履歴

図 6 速度履歴

3.3 考察

GPS の補正が得られる位置や速度については NAV420 とほぼ一致することが確認された。また GPS の補正がない姿勢についても、 NAV420 と比較してロールは一致、ピッチはオフセットを除けば一致、ヘディングは傾向が一致というよい結果が得られた。センサのキャリブレーションを行えばより近い精度が得られるものと思われるが、実験にて本システムの有効性が示せたと思われる。

MEMS センサを用いた INS 単体では 1 分程度でも誤差がたまってしまい全く実用にならないことから考えると、 GPS による補正がシステム全体の精度に大きく貢献しているものと思われる。また直接 GPS の観測量が得られない姿勢については、ロール・ピッチに

図 7 姿勢履歴

比べ、ヘディングの結果がよくないが、これは前者が運動とのカップリングが強いのに対し、後者が弱いためだと思われる。飛行機などの進行方向が定まっているアプリケーションについては速度方向を意図的に補正時に組み込むことにより、ヘディングの精度を改善できるであろう。 4 まとめと今後の課題

本研究で提唱したシステムの有効性を実験により示すことができた。しかし、実用化に向けては長時間使用におけるセンサのドリフト変動や GPS の使用可能性などが与える影響をより詳しく検証する必要があるだろう。また航空機は運動が連続的でありよい結果が得られやすい対象である。激しい運動を行う人やロボットなどの対象についても今後検証を行いたい。参考文献

[1] Robert M. Rogers. Applied Mathematics in Integrated Navigation System, Second Edition. AIAA Education Series, 2003. ISBN 1-56347-656-8.

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

[3] 片山徹『新版応用カルマンフィルタ』朝倉書店, 2000. ISBN 4-254-20101-X

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