1analog.com/jp/analog-dialogue

IMUと地磁気センサーを組み合わせて、ストラップダウン方式の慣性ナビゲーション・システムを構築著者: Joel Li、Van Yang

この方式では、プラットフォーム方式のナビゲーション・システムが抱える欠点が解消されます。但し、多くの場合、S I N Sを使うよりもプラットフォーム方式のナビゲーション・システムを使う方が高い精度が得られます。通常、プラットフォーム方式のナビゲーション・システムでは、ストラテジック・グレード（ジャイロ・センサーのバイアス： 0 . 0 0 0 1 ° /時、加速度センサーのバイアス：1 μg）または軍用グレード（ジャイロ・センサーのバイアス： 0 . 0 0 5 ° /時、加速度センサーのバイアス：30μg）の性能が得られます。それに対し、ほとんどのS I N Sは、ナビゲーション・グレード（ジャイロ・センサーのバイアス：0 .01° /時、加速度センサーのバイアス：50μg）またはタクティカル・グレード（ジャイロ・センサーのバイアス：10° /時、加速度センサーのバイアス：1mg）しか達成できません。とはいえ、それだけの精度があれば、ほとんどのサービス用ロボットや無人搬送車（AGV：Automated Guided Vehic le）で必要とされるナビゲーション機能に十分に対応できます。

ナビゲーションの実現手段としては、マシン・ビジョン、GPS、UWB（Ul t r a Wide Band）、LIDAR（Ligh t D e t e c t i o n a n d R a n g i n g） 、 S L A M（ S i m u l t a n e o u s Loca l iza t ion and Mapping）など、様々な技術が使われています。何らかのナビゲーション・システムにおいて、慣性ナビゲーションが重要な要素である場合には、IMUが使用されます。しかし、この種のセンサーには、バイアス誤差、直交軸誤差、ノイズ、更にはバイアスの不安定性といった制約があります。そのため、慣性ナビゲーションでは、通常、リファレンス用／キャリブレーション用の情報を定期的に供給するセンサーを併用する必要があります。これをセンサー・フュージョンと呼びます。センサー・フュージョンでは、カメラやオドメータ（走行距離計）など、多様なセンサーが IMUと共に使用されます。それらのセンサーの中でも、地磁気センサーは、 IMUと併用して姿勢を検出するための低コストの手段となります。

アナログ・デバイセズは、 IMUとしてADIS16470を採用し、地磁気センサーを組み合わせたプラットフォームを開発しました。本稿では、そのプラットフォームを使用してSINSを実現するためのアルゴリズムを紹介します。但し、地磁気センサーは姿勢情報しか提供しません。デッド・レコニングや距離の測定には、 IMUが内蔵する加速度センサーだけを使用します。

概要

本稿では、ストラップダウン方式の慣性ナビゲーション・システム（S I N S：S t r a p d o w n I n e r t i a l N a v i g a t i o n S y s t e m）を実現する方法を紹介します。その主要な構成要素は、アナログ・デバイセズの慣性計測ユニット（ I M U： I n e r t i a l M e a s u r e m e n t U n i t）センサー「ADIS16470」とPNI Sensor製の地磁気センサー（コンパス）「RM3100」です。開発したシステムには、SINSを実現するためのいくつかの基本的なプロセスを実装しています。具体的には、コンパスのキャリブレーション、拡張カルマン・フィルタ（EKF：Extended Kalman Fi l ter）を用いた姿勢方位基準システム（A H R S：A t t i t u d e a n d Heading Reference Sys tem）、トレースのトラッキングなど、磁気／角速度／重力（MARG：Magnetics , Angular Rates, and Gravity）に基づく処理です。また、最小2乗誤差（MSE：Minimum Squared Error）法を用いた疎結合によるセンサー・フュージョンも実現されています。本稿では、各プロセスのステップにおいて使用するアルゴリズムについて、特に詳しく説明します。その上で、開発したシステムをテストするための実験環境、実験結果の分析、精度を改善するための方法について解説を加えます。

はじめに

現在は、各種のサービスを提供するロボット製品の市場が拡大すると共に、関連技術が著しく進化している状況にあります。それに伴い、ナビゲーション技術の研究と応用に対して、多大な関心が寄せられるようになりました。自動車、船舶、航空機と比べると、サービス提供用のロボットは小型で低コストの製品です。このことから、その種のロボットに搭載するナビゲーション・システムは、ストラップダウン方式を採用してコストを低く抑えたものでなければなりません。安定したプラットフォーム方式を採用した従来のナビゲーション・システムは、通常、独立した加速度センサーと、ファイバまたはレーザをベースとするジャイロ・センサーを搭載しています。この方式では、すべてのコンポーネントが、可動車両から分離された安定したプラットフォーム上に機械的にしっかりと固定されます。この手法には、サイズが大きく、信頼性が低く、コストが高いという欠点があります。一方、SINSでは、慣性センサーが車両本体に直接取り付けられます。つまり、同センサーが車両と共に回転するということです。

Analog Dialogue 53-03、2019年3月Share on Twitter LinkedIn Facebook Email

Analog Dialogue 53-032

ADIS16470の概要

A D I S 1 6 4 7 0は、 3軸ジャイロ・センサーと 3軸加速度センサーを搭載するM E M S（M i c r o E l e c t r o M e c h a n i c a l Sys t ems）ベースの小型 IMUです。ジャイロ・センサーは8 ° /時、加速度センサーは1 3 μ gというバイアスの安定性を備えています。主要なパラメータについては、工場から出荷する際にキャリブレーションが適用されます。同等レベルの製品と比べて安価であるため、多くの顧客に広く採用されています。本稿の例では、ADIS16470との通信は、マイクロコントローラを使用し、SPI（Ser ia l Per iphera l In te r face）を介して行います。

地磁気センサーの概要

地磁気センサーは、方位の絶対基準であるコンパス機能のボディ座標系（フレーム）に対する地球の磁場を測定するために使用します。X/Y/Z座標の値は、局所的な地球の磁場からの投影として得られます。この種のセンサーには、2つの大きな欠点があります。1つは、精度と分解能を高くできないことです。例えば、よく使用されるコンパスの例にHoneywellの「HMC5883L」があります。その分解能はわずか12ビットです。もう1つの欠点は、地球の磁場が数ミリガウス～8ガウスと非常に弱いことから、センサーに対し、周辺環境からの干渉の影響が簡単に及んでしまうことです。

このような欠点はあるものの、地磁気センサーは、広々とした土地やEMI（電磁干渉）の少ない環境など、多くの条件下で十分に機能します。また、 IMUとの間で疎結合を実現すれば、ほとんどの環境で地磁気センサーを使用することができます。

本稿で使用するRM3100は、24ビットの分解能を備える高性能のコンパスです。同製品には、ノイズ耐性を高めるためにアクティブ励起手法が適用されています。

コンパスのキャリブレーション

コンパスを使用する場合、事前にキャリブレーションを実施し、2つの主要な誤差を除去しておく必要があります。1つは、センサーと回路のオフセット誤差に起因するオフセット誤差です。もう1つは、スケール誤差です。どちらの誤差も、周囲の磁場環境からの干渉によって簡単に変動してしまいます。例えば、X軸方向の外部磁場をセンサーに印加すると、X軸方向の外部オフセット誤差が加わります。同時に、X軸のスケールも、Y軸、Z軸のスケールと比べて差異のある状態になります。

磁気センサーに対する一般的なキャリブレーション手法では、XY平面上で円を描くようにセンサーを回転させてデータをプロットします。ある特定の場所において地球の磁場の強度は一定なので、描画されるデータは円になるはずです。しかし、実際に描画されるのは楕円です。したがって、楕円の移動／再スケールを行ってゼロを中心とする円になるよう補正を実施する必要があります。

上述した2D（2次元）のキャリブレーション手法には、いくつかの欠点があります。また、傾斜を測定するための加速度センサーが必要になります。ここでは、3Dの球体フィッティングによってコンパスのキャリブレーションを行う方法を紹介します。その方法では、まずセンサーをX/Y/Z空間の全方向に回転させて、その値を3D座標にプロットします。次にMSE法を用い、データを楕円体としてフィッティングします。

コンパスの未処理のデータ

3k

2k

2k

2k

1k

1k

1k

–1k

–1k–1k

–2k

–2k–2k

–3k

–3k –3k

0

00

X軸Y軸

Z軸

3k

2k

2k

2k

1k

1k

1k

–1k

–1k

–1k

–2k

–2k

–2k

–3k

–3k–3k

0

0

0

楕円体のフィッティング

X軸

Y軸

Z軸

図 1 . コンパスのキャリブレーション。左はコンパスから得られた未処理のデータの分布を表しています。

右は楕円体に対してフィッティング処理を適用した後のデータです。

Analog Dialogue 53-03 3

楕円体は次の式で表されます。

aX2 + bY2 + cZ2 + 2fXY + 2gXZ + 2hYZ + 2pX + 2qY + 2rZ + d = 0 (1)

ここで、X、Y、Zは、コンパスの出力として得られる3軸方向の地球の磁場成分です。これらの値を楕円体にフィッティングするには、最適な一連の係数値が必要になります。ここでは、係数を次のように定義します。

σ = [a b c f g h p q r d ]T

また、フィッティングの際のベクトルを次のように定義します。

β = [X2 Y2 Z2 2XY 2XZ 2YZ 2X 2Y 2Z ]T

ここで、最適なσを計算し、以下の式（2）によって最小値を求めます。

min (σT βT βσ) (2)

以上によって、図1に示したフィッティング結果が得られます。

センサーのキャリブレーションを行うには、フィッティング後の楕円体を伸長／移動し、ゼロを中心とする球体にする必要があります。ここでは、行列の特異値分解（SVD：Singular Value Decomposi t ion）法を用いてそのキャリブレーションを行います。図2に示したのが、キャリブレーションによって得られた球体です 1、2。

キャリブレーションによって得た球体

3k

2k

2k

2k3k

1k

1k

1k

–1k

–1k–1k

–2k

–2k–2k

–3k

0

00

X軸Y軸

Z軸

図 2 . S V D法によるキャリブレーションの結果。 コンパスからのデータが球体になるよう補正しています。

図3に示すように、キャリブレーションの実施後には、磁界強度の測定値（球の半径）がほぼ一定の値に保たれることがわかります。

3000

3800

3600

3400

3200

2800

2600

2400

2200

4000

2000 25001500 2000500 1000 3000 3500 4000 4500

半径〔LS

B〕

ポイント数

磁界強度（デジタル化後）

キャリブレーションの実施前キャリブレーションの実施後

図 3 . キャリブレーション前後の磁界強度の比較

ADIS16470とコンパスで構成したAHRSAHRSは、ロール、ピッチ、ヨーといった姿勢の情報を供給する3軸上のセンサーで構成されます。航空機の航法に関する概念であるAHRSは、姿勢（方向）を表現するために使用されます。

本稿で紹介するシステムについて理解していただくために、まずは姿勢の検出にセンサー・フュージョンが必要になる理由を説明します。実際、本稿で示すシステムでは、ジャイロ・センサー、加速度センサー、コンパスという3種類のセンサーを使用します。

ジャイロ・センサーは、各軸に対する回転の角速度を検出します。角速度を積分すると回転角が得られます。初期方位が既知であれば、必ず方位姿勢の値を得ることができます。但し、積分によってジャイロ・センサーの不安定なバイアスが蓄積され、それが角度の誤差につながります。また、ジャイロ・センサーから生じるガウス分布のノイズがブラウン運動のプロセスに作用して、ランダム・ウォーク誤差が発生します。そのため、ジャイロ・センサーを長時間使用し続けるのは困難であり、定期的にキャリブレーションを実施する必要があります。

加速度センサーは、各軸の方向に生じる動きの加速度を検出します。静的な状態では、各軸に対する重力加速度の方向の角度を取得することが可能です。重力加速度の向きと値は一定なので、重力の方向に対する方位姿勢を求められます。しかし、この方法は重力加速度を基準とするので、重力加速度の方向の回転角を求めることはできません。

コンパスは、地球の磁場から投影される各軸の値を出力します。各軸と地球の磁場の方向（これも一定のベクトルです）の関係から、角度の値を算出することができます。先述したように、コンパスは外部の磁場に対する耐性が低いので、干渉の少ない環境で使用する必要があります。

以上の内容から、1つのセンサーだけを頼りに姿勢を検出するのは難しいことがわかります。2つまたは3つのセンサーを組み合わせて使用し、情報を融合しなければならないということです。そのため、本稿の例では、加速度センサーとジャイロ・センサー、コンパスを使って姿勢を検出します。この種のセンサー・フュージョンを実現するものをMARGシステムと呼びます。

Analog Dialogue 53-034

EKFの設計とセンサー・フュージョン

IMUとコンパスからのデータの融合に使用する方法としては、相補フィルタ、統計ARMA（Autoregress ive Moving Average Model）フィルタ、カルマン・フィルタなどが挙げられます。本稿の例ではEKFを使用します。

まずは、本稿で使用するいくつかの定義を示します。

座標の定義

方位または方向は、 2つの座標系（フレーム）の間の関係として表されます。一方の座標は常に変化し、他方は一定です。座標の定義方法としては、ナビゲーション座標系（航法座標系）とボディ座標系を使用します。NED（Nor th -Eas t -Down：北 -東 -下）座標系や地理学的な手法とは異なり、ナビゲーション座標系として測定されたボディ座標系の初期値を定義します。ナビゲーション座標はその後一定になります。ボディ座標系からナビゲーション座標系へのマッピング（投影）用の行列を次のように定義します。

nCb

姿勢の定義

オイラー角やDCM（Direc t ion Cos ine Matr ix：方向余弦行列）とは異なり、ここでは四元数qを定義します。四元数は、偏角を避けるためにナビゲーション・システムで一般的に使用されています 3。

カルマン・フィルタを用いた姿勢の更新

本稿で使用する運動方程式（つまり状態遷移方程式）は偏差形式であり、線形ではありません。このことから、E K Fを使用するために、偏差方程式に 1次線形近似を適用する必要があります。EKFの設計に向けて、次のように定義します。

x = [ω q ]T

これは、状態変数を表す1×7のベクトルです。

以下の式は角速度を表します。

ω = [ωx ωy ωz]T

以下の式は姿勢の四元数です。

q = [a b c d ]T

以下の式は、観測変数を表す1×7のベクトルであり、要素は状態変数と同じです。

z = [ω q ]T

以下の式は、7×7の状態遷移行列です。

A = 1T

(ωk – ωk – 1) 12

qk – 1ωk – 1

第1項はデジタル化した角速度の微分方程式、第2項はデジタル化した四元数の更新式（運動方程式から導出）です。

以下に示すのは、7×7の観測行列です。

10

01H =

以下に示すのは、7×7の誤差共分散行列です。

P = E[eT e]

ここで、 eは実測値xと推定値x ̂の誤差を表すベクトルです。実験では、初期の誤差としては比較的小さい値を設定します。これは、自動的に小さな値に収束します。

RとQは、状態遷移に伴うノイズと観測したノイズのノイズ共分散です。その初期値であるR0とQ0は、ジャイロ・センサーと加速度センサーのAC RMS値の2乗を測定することによって得られます。IMUとコンパスを静的な状態に維持し、次のように設定します。

R0 = Q0

以上のような定義に基づき、次の5つのステップによってカルマン・フィルタを実現します。

ステップ1：カルマン・ゲインKを式（3）によって計算します。

Kk = Pk HT (HPk HT + Rk)– – (3)

ステップ2：誤差共分散行列Pを計算します。

Pk = (I – Kk H) Pk– (4)

ステップ3：推定状態x ̂を出力します。

xk = xk + Kk(zk – Hkxk )–ˆ ˆ ˆ – (5)

ステップ 4：状態 x ̂ –を投影します。

xk + 1 = Axk–ˆ ˆ (6)

ステップ5：誤差共分散行列P –を投影します。

Pk + 1 = APkAT + Qk–

(7)

Analog Dialogue 53-03 5

図4のブロック図は、このプロセスを簡略化して示したものです。

ω.

q

ω

ωz =

qx –

q

.ω. dω

dt=

12 qω=q.

x0 = [ω0 q0] x –x x –x= + K(z – H x )

ω= qx∫

図 4 . 姿勢更新のために カルマン・フィルタで行う処理

MSE法に基づくセンサー・フュージョン

上のセクションで、観測変数を次のように定義しました。

z = [ω q ]T

これには、コンパスからの情報が含まれていません。ωは角速度であり、コンパスからのデータqのインポートには四元数しか使用できません。以下、MSE法を用いて、観測変数の要素であるqを求める方法を示します。

まず、以下のように各種の変数を定義します。

m b、a b：ボディ座標におけるコンパスの磁気の値と加速度の値。

m n、a n：ナビゲーション座標におけるコンパスの磁気の値と加速度の値。

m n0、a n0：ナビゲーション座標におけるコンパスの磁気の初期値と加速度の初期値。

以下に示すのは、四元数を用いて表したボディ座標からナビゲーション座標への姿勢変換行列です。

nCb

これは、次のように記述できます。

(q)nCb

これにより、ナビゲーション座標の初期値と、ナビゲーション座標からボディ座標へリアルタイムにマッピングされた値の間の誤差εが得られます。

先述した定義に基づき、MSE法を用いて以下の式の最適な値を求めることができます。

(q)nCb

最適になるのは、以下の式（8）の値が最小になるときです。

(8)an0mn0

nf (q) = ɛT ɛ = T– Cb(q) abmban0mn0

n– Cb(q) abmb

ここで f (q)を微分結果がゼロになるとします。

= 0f(q)q

(9)

これにより、分散の意味で最適なqが得られます。続いて、ガウス・ニュートン法を使用し、1次勾配収束を示す上記の非線形方程式を解きます。

角速度を組み合わせることにより、以下の観測変数が得られます。

z = [ω q ]T

これにより、コンパスのデータと IMUのデータがカルマン・フィルタによって融合されます。

図5は、以上のプロセスを簡略化して示したものです。

ω

ωz = q

mb

ab

ω

qmin(f(q)) = min(εTε) = – –(q)an0mn0

an0mn0

nb

abmb

abmb

TC (q)nbC

df(q)dqmaking = 0

図 5 . M S E法による センサー・フュージョンの処理

疎結合

先述したように、コンパスを使用できない状況に遭遇するケースがあります。干渉を受けた状態で磁気のデータを取得した場合、得られる姿勢の精度は IMUのみを使用する場合よりも低くなります。そこで、磁気センサーを使用できるかどうかを判断する疎結合を採用することにしました。図6に示すように、磁気センサーを使用できない場合、IMUだけを使用して姿勢の値を検出します。一方、磁気センサーが使用できる場合には、センサー・フュージョンのアルゴリズムを適用して姿勢の値を検出します。

姿勢

ジャイロ・センサーに基づく手法

カルマン・フィルタによるフュージョン

磁気センサーを使用できるか

センサーからの値の読み取り

はい

いいえ

いいえ

はい

システムは動いているか

図 6 . 姿勢の値を得るための演算の流れ

Analog Dialogue 53-036

新たなデータを取得した後、または新たに姿勢の値を演算して取得するサイクル（以下、決定サイクル）において、加速度の大きさを計算します。決定サイクルとサンプリング・サイクルが異なるシステムもありますが、ここでは1つのサンプリング・サイクルで姿勢の値を決定すると仮定しています。加速度が1gでない場合、姿勢の値の計算には加速度センサーの出力は使用しません。次に、コンパスからの出力の大きさを計算し、初期値と比較します。両者の値が等しくない場合、このサイクルでは地磁気センサーからのデータは使用しません。両方の条件が満たされる場合には、カルマン・フィルタを使用してMSEによるフュージョンの処理を行います。

ADIS16470によるデッド・レコニング

ナビゲーション・システムでは、前回特定された位置を使用する決定サイクルにおいて、既知または推定される速度または加速度に基づいてその位置を移動させることにより、現在の位置を計算します。このプロセスをデッド・レコニング（DR：Dead Reckon ing）と呼びます。ここでは、加速度センサーとしてADIS16470を使用しています。前のセクションで算出した姿勢の値に基づき、SINSの移動方向を検出します。続いて、その方向への距離を計算することにより、最終的な位置を決定します。

DR手法の概要

ストラップダウン方式におけるD R手法では、加速度の測定に基づく外力（Spec i f i c Fo rce）の式を用いて、慣性ナビゲーション・システムの位置をトラッキングする必要があります。外力の式は、以下に示すように簡単に表現できます。

ae(t) = Cb ab(t)e (10)

ve(t + δt) = ve(t) + δt × (ae(t + δt) – ge) (11)

se(t + δt) = se(t) + δt × (ve(t + δt)) (12)

ここで、 a eは地球座標系における加速度、 a bはボディ座標系における加速度、v eは地球座標系における速度、 s e

は地球座標系における距離です。g eは地球座標系における重力加速度であり、gを単位とするベクトル [0 0 1 ]です。ここで、地球座標系はナビゲーション座標系とは異なり、NED座標であることに注意する必要があります。δtは値の決定サイクルです。

式（10）は、以下の部分からわかるように、 IMUのボディ座標系から地球座標系に対して加速度を投影します。

eCb

式（11）では、加速度を積分または累積して速度を求めます。但し、加速度の測定値には重力が含まれているので、その成分を差し引く必要があります。

式（11）と同様に、式（12）では速度を積分して距離を求めます。

従来の方法には、以下に挙げる複数の問題が存在します。

X 加速度の出力には必ずバイアスが存在し、それが重力と組み合わせられます。そのため、式（10）での除去は難しくなります。より正確な式は次のようになります。

an ee

aee

ade= Cb –

axb

ayb

azb

axbias

aybias

azbias–

00g

(13)

分割ヘッダなどの専門的な装置を使用しない限り、除去を実現するのは困難です。

X 一般に、従来の数値の積分方法では、0次ホールド（1つ前の値）が使用されます。しかし、この方法では連続した動作については大きな誤差が生じてしまいます。例として、以下の2つの方法を比較してみます。

方法1：

vi = vi – 1 + ai – 1t

（0次ホールド）

方法2：

vi = vi – 1 + ai – 1t +ai – ai –1

2

（線形補間）

Analog Dialogue 53-03 7

2

1

5

9

8

7

6

4

3

10

00 2.5 3 3.5 4 4.50.5 1 1.5 2 5

速度〔

m/秒〕

速度

時間〔秒〕

0次のS/H1次のS/H

2

6

10

8

4

12

00 2.5 3 3.5 4 4.50.5 1 1.5 2 5

距離〔

m〕

変位

時間〔秒〕

0次のS/H1次のS/H

図 7 . 速度の計算における 0次と 1次の積分方法の比較

0 .5m/秒 2の加速度が5秒間続くと、最大で4mの変位が生じます。図7に、上記の内容に関するシミュレーション結果を示しました。

ここまでの議論に基づき、従来の外力の式に次の2点の変更を加えて、アプリケーションに適用することにしました。

X 地球座標はナビゲーション座標系としては使用しません。代わりに、1つ前の姿勢の値を検出したときと同様に、以下の初期姿勢をナビゲーション座標系として使用します。

nax0 nay0 naz0

これにより、式（14）に示すように、バイアスと重力の両方を簡単に除去することができます。

ax nn

ayn

azn= Cb –

axb

ayb

azb

ax0n

ay0n

az0n (14)

初期姿勢にはバイアスと重力の成分が含まれていますが、この方法であれば各成分を区別する必要はなく、直接除去することが可能です。

X 0次ホールドと 1次補間を比較した結果、より正確な積分結果が得られた1次補間を採用することにしました。

運動パターンとゼロ速度更新技術

IMUで得た初期値をナビゲーション座標系の値として使用すれば、加速度センサーの初期バイアスの影響を部分的に除去することができます。しかし、デバイスを使用する前に、分割ヘッダを使ってバイアスを正確に測定できたとしても、その除去はやはり容易ではありません。したがって、別の高精度のセンサーを利用して定期的にキャリブレーションを実施する必要があります。これに関する主な誤差要因は2つにあります。1つはバイアスの不安定性です。

つまり、事前に測定したバイアスは、現時点の実際のバイアスとは異なるということです。もう1つは速度のランダム・ウォークです。加速度にはこれが必ず伴います。前述した望ましくない特性により、距離の計算値には大きなずれが生じます。動きを停止して静的な状態になっても、加速度に基づく速度はやはり存在し続け、距離はやはり増加します。

こ の 問 題 を 解 決 す る に は 、 ゼ ロ 速 度 更 新 技 術（ZUPT：Zero Speed Updat ing Technology）によって速度をリセットする手段を適用する必要があります。ZUPTはアプリケーションに密に依存します。そのため、システムとアプリケーションの運動特性を取得し、アルゴリズムに規則を与える必要があります。より多くの運動パターンを検出するほど、結果は正確になります。

開発したSINSのテストは、それを設置した回転いすを動かすことによって行う予定でした。この研究は、特定のアプリケーションに限定されるものではないので、運動については以下のように仮定しました。

X D Rにおいて、ナビゲーション座標系ではZ軸方向の動きはありません。この制約はD Rだけに対するもので、姿勢の決定には適用されません。つまり、システムは2D間で動きます。これにより、Z軸方向の誤差を排除することができます。

X 回転は、必ず停止後に生じます。動いている最中に回転が生じると、追加の加速度が加わるため、姿勢の決定の際に干渉が生じます。

X システムが動いている場合、加速度を一定に保てるのは500ミリ秒までです。また速度を一定に保てるのは2秒までです。回転いすを押したり引いたりしますが、500ミリ秒を超える時間、手によって力を正確かつ一定に保つのは困難です。また、人間が2秒以上一定の速度で回転いすを押し続けることも容易ではありません。実際、ZUPTを実行するにあたり、この規則を適用しています。

X 加速度を ± 1 m /秒 2よりも大きくすることはできません。この規則は、ノイズに対するフィルタリングに適用されます。フィルタリングは、いすを押す力または引く力に基づきます。その力は大きいものではありません。

Analog Dialogue 53-038

図8からわかるように、システムがX軸方向に動いているとき（ナビゲーション座標系への投影後）、Y軸方向にも加速度が生成され、積分後のY軸方向の速度はゼロにはなりません。つまり、X軸方向にしか動いていない場合でも、DRシステムはY成分の存在を示すことになります。

0.1

–0.1

0

X軸〔

m/秒

2 〕

200 400 600 800 1000

ナビゲーション座標系における加速度

1200 1400 1600 1800 200000.1

–0.1

0

Y軸〔

m/秒

2 〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0

Z軸〔

m/秒

2 〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0.05

–0.05

図 8 . ナビゲーション座標系における 3軸方向の加速度

運動に関する3つ目の仮定に基づくと、ZUPTを適用することによって、この誤差を除去できます。Z U P Tを適用した後、積分によって得られた速度を図9に示します。

1

–1

0

1

–1

0

1

–1

0

X軸〔

m/秒〕

200 400 600 800 1000

ナビゲーション座標系における速度

1200 1400 1600 1800 20000

Y軸〔

m/秒〕

200 400 600 800 1000 1200 1400 1600 1800 20000

Z軸〔

m/秒〕

200 400 600 800 1000 1200 1400 1600 1800 20000

図 9 . ナビゲーション座標系における 3軸方向の速度

3つ目の仮定を適用していますが、誤差を完全に除去することはできていません。誤差の除去は、加速度のゼロと速度のゼロの判定に使用する閾値に依存するからです。とはいえ、ほとんどの誤差は補正されています。

ベースラインのシフトの補正

ZUPTを適用しても、加速度をゼロにできない場合があります。その原因は次の2つです。

X ZUPTを適用しても、バイアスの不安定性による誤差と、速度のランダム・ウォークを完全に除去することはできません。

X 決定済みの姿勢の値にはいくらかの誤差があります。そのため、（ボディ座標系からナビゲーション座標系への）投影後の加速度にも誤差が含まれます。

例として、図 1 0について見てみます。この図の左側は、A D I S 1 6 4 7 0から得た未処理のデータ（ボディ座標系）をプロットしたものです。一方、右側に示したのは、ナビゲーション座標系に投影された加速度です。動きを停止している場合でも、投影後の加速度はゼロではないことがわかります。このように、常に加速度が変化する現象をベースラインのシフトと呼びます。

ベースラインのシフトを補正するには、バイアスのシフト分をリアルタイムかつ連続的に取得し、投影後の加速度の値から差し引く必要があります。そのように処理した結果を図11に示しました。

0.1

0.05

0

–0.05

–0.1

X〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

処理前の加速度

0.1

0.05

0

–0.05

–0.1

X〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

処理後の加速度

バイアスのシフト分処理後の加速度

図 1 1 . ベースラインのシフトを補正する前後の加速度。

上が補正前で、下が補正後の結果です。

図11の上のグラフは、ベースラインのシフトを補正する前の加速度を表しています。下のグラフは補正後の結果です。緑色のプロットは計算によって得たベースラインのシフト分、赤色のプロットはベースラインのシフトを補正した後の加速度です。

0.1

0.05

–0.05

0X〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0.1

–0.01

0

Y〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0.1

–0.01

0

Y〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0.1

–0.01

0

X〔g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

–0.05

0

Z〔

g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

1.05

1

–1.05

Z〔

g〕

200 400 600 800 1000 1200 1400 1600 1800 20000

ナビゲーション座標系に投影した後の加速度未処理の加速度データ

0.05

図 1 0 . ボディ座標系（左）と ナビゲーション座標系（右）における加速度

Analog Dialogue 53-03 9

図12に、DRの処理の流れを簡素化して示しました。ボディ座標系の加速度a bと、以下に示す（AHRSに基づく）姿勢変換行列をDRシステムに入力します。

nCb

これにより、ナビゲーション座標系における位置の値が得られます。

ベースラインのキャリブレーション

ZUPT

加速度のゼロ判定

∫vn = an × dt ∫sn = vn × dt

an = Cab(ab – gb)

nb

C nb

図 1 2 . D Rの処理の流れ

実験結果、まとめ

実験結果

図13に示したのが、実験に使用したシステムの構成図です。SPIポートを使って、ADIS16470の評価用ボードとRM3100の評価用ボードを、アナログ・デバイセズのマイクロコントローラ「ADuCM4050」の評価用ボードに接続しています。

ADuCM4050により、データの形式を調整すると共に、時間同期を確立します（ I M Uとコンパスのデータ・レートが異なるため）。取得したデータは、 U A RTを介してコンピュータに送信されます。キャリブレーション、AHRS、DRを含むすべての演算は「MATLAB ®」で実行します。

ADuCM4050の評価用ボード

SPI

UART

SPI

コンピュータ

ADIS16470の評価用ボード

RM3100の評価用ボード

図 1 3 . 実験用システムの構成

実験では、すべてのボードとコンピュータを回転いすの上に配置しました。そして、実験室において円を描くように回転いすを回転させて測定を行いました。

X AHRSの出力：四元数とDCMのそれぞれの形式で示した姿勢の値を図14に示しました。

X D Rの出力：X / Y / Z座標における位置と 3 Dグラフで示したDRの結果を図15に示しました。

0.05

0

–0.05

四元数 DCM

200 400 600 800 1000 1200 1400 1600

1000 20000

0

2DCM11

–21000 20000

0

2DCM12

–21000 20000

0

0.1DCM13

–0.1

1000 20000

0

2DCM21

–21000 20000

0

2DCM22

–21000 20000

0

0.05DCM23

–0.05

1000 20000

0

0.1DCM31

–0.11000 20000

0

0.05DCM32

–0.051000 20000

0

1.05DCM33

1

1800 20000

a

0.01

0

–0.01200 400 600 800 1000 1200 1400 1600 1800 20000

b

2

0

–2200 400 600 800 1000 1200 1400 1600 1800 20000

c

2

0

–2200 400 600 800 1000 1200 1400 1600 1800 20000

d

4

2

0X〔m〕

200 400 600 800 1000 1200 1400 1600 1800 20000

–5

0

Y〔m〕

200 400 600 800 1000 1200 1400 1600 1800 20000

0.01

0

–0.01

Z〔m〕

200 400 600 800 1000 1200 1400 1600 1800 20000

ナビゲーション座標系における位置

Y軸位置〔m〕X軸位置〔m〕

22

3

4

11

00

トレースのトラッキング

–2

5

図 1 5 . 位置の算出結果

図 1 4 . 姿勢の値を四元数とD C Mの各形式で表した結果

Analog Dialogue 53-03 10

参考資料1 L o n g L i 、 Z h a n g H e 「 A u t o m a t i c a n d A d a p t i v e Cal ibra t ion Method of Tr i -axia l Magnetometer（3軸磁気センサーの適応型自動キャリブレーション手法」Chinese Journa l of Sc ien t i f ic Ins t rument、2013年

2 Timothy Sauer「Numer ica l Analys is (2nd Edi t ion)（数値解析 第2版）」Pearson、2011年

3 D a v i d H . Ti t t e r t o n「S t r a p d o w n I n e r t i a l N a v i g a t i o n Te c h n o l o g y ( 2 n d E d i t i o n )（ストラップダウン方式の慣性ナビゲーション技術 第 2版）」T h e I n s t i t u t i o n o f Elec t r ica l Engineers、2004年

Ya n G o n g m i n「 R e s e a r c h o n Ve h i c l e A u t o n o m o u s P o s i t i o n i n g a n d O r i e n t a t i o n S y s t e m（自律型のポジショニング／オリエンテーション・システムの研究）」Ph.D. thes i s . Nor thwes tern Poly technica l Univers i ty、中国、2006年

J o ã o L u í s M a r i n s「 A n E x t e n d e d K a l m a n F i l t e r f o r Quatern ion-Based Or ien ta t ion Es t imat ion Using MARG Sensors（MARGセンサーを用いた四元数ベースの方向推定向け拡張カルマン・フィルタ）」 IEEE、2001年

I g o r P. P r i k h o d k o、 B r o c k B e a r s s「 To w a r d s S e l f -N a v i g a t i n g C a r s U s i n g M E M S I M U : C h a l l e n g e s a n d Oppor tuni t ies（MEMSベースの IMUを利用する自走車両に向けて：課題とチャンス）」 IEEE、2018年

RM3100、PNI Sensor Corpora t ion、2018年

O l i v e r J . Wo o d m a n「 A n I n t r o d u c t i o n t o I n e r t i a l Naviga t ion（慣性ナビゲーションの概要）」Unive r s i t y of Cambr idge、2007年8月

まとめ

本稿では、アナログ・デバイセズの IMU（ADIS16470）と地磁気センサー（RM3100）を組み合わせてSINSを構築するための基本的なプロセスを説明しました。そのなかでも、キャリブレーション、AHRS、DRについては特に詳しく解説を行いました。実験環境などの条件に制約があるため、開発したプラットフォームとアルゴリズムについて更に詳しくテストするのは困難です。ただ、いくつかの手法を適用することにより、更なる改善を図ることができます。例えば、以下のような手法です。

X オドメータやU W Bを利用して距離を測定することにより、加速度センサーと I M Uを融合し、D Rにおける距離の精度を改善することができます。

X A H R SとD Rにおいて、システムの振動、加速、減速のモデル、地面の平坦性など、センサーのレベルやシステムのレベルでより多くの特性を取り入れた、より複雑な運動モデルを使用します。その結果、更に多くの境界条件が計算に加わり、より正確な結果が得られるようになります。

X シンプソンの公式や3次スプライン補間など、より正確な数値計算方法によってD Rの積分を行います。あるいは、ガウス・ニュートン法の代わりにニュートン法を使い、非線形のMSE方程式を解きます。これらの手法により改善が図れます。

実験によって判明した最も重要な点は、慣性ナビゲーション・システムは、アプリケーションまたは運動パターンに非常に密に関連するということです。例えば、カーペットが床に敷かれていない実験室とカーペットが敷かれたオフィスの2ヵ所で実験を行ったところ、同じパラメータ集合を使ったDRの結果には大きな差が生じました。つまり、患者の追跡、AGVのナビゲーション、駐車スペースの位置の特定など、どのようなアプリケーションに適用する場合にも、その運動モデルを包括的に理解することが必要になります。同一のアプリケーションで条件が異なる場合にも同じことが言えます。

著者：

Joe l L i (Zhao L i )（ j oe l . l i@ana log .com）は、アナログ・デバイセズのアプリケーション・エンジニアです。 2 0 1 7年に入社しました。現在は中国で、 I M UやL I D A Rを利用するアプリケーションに取り組んでいます。2013年に中国科学院で光電検出に関する修士号を取得しています。

Joel Li

Van Yang（van .yang@analog .com）は、アナログ・デバイセズの中国上海支社に所属するフィールド・アプリケーション・エンジニア（FAE）です。2015年に入社し、医療分野や産業分野に携わる中国の顧客をサポートしています。アナログ・デバイセズに入社する前は、Texas Instruments社のFAEとして4年間勤務していました。2011年に武漢にある華中科技大学で通信／情報システムに関する修士号を取得しています。バスケットボールの大ファンで、余暇にはハイキングを楽しんでいます。

Van Yang