このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

慣性センサー フュージョンによる方向の推定

R2026a
ライブ スクリプトを開く

この例では、6 軸および 9 軸のフュージョン アルゴリズムを使用して方向を計算する方法を示します。慣性計測ユニット (IMU) と磁力-角速度-重力 (MARG) ユニットから方向を計算するためのアルゴリズムはいくつか存在します。この例では、方向の基本とこれらのアルゴリズムの使用方法について説明します。

方向

オブジェクトの方向は、3 次元において、ある座標系 (親座標系と呼ばれることもある) に対する回転を表します。

以下のアルゴリズムでは、固定された親座標系として北-東-下 (NED) を使用します。NED は、グローバル座標系または基準座標系と呼ばれることもあります。NED 基準座標系では、X 軸は北を指し、Y 軸は東を指し、Z 軸は下を指します。NED の X-Y 平面は、地球の局所接平面と見なされます。アルゴリズムによって、北は磁北または真北のいずれかになります。この例のアルゴリズムでは、磁北を使用します。

東-北-上 (ENU) 親座標系を指定した場合、以下のアルゴリズムは、NED の代わりに、ENU 親座標系を基準とした方向を推定します。

オブジェクトは、独自の座標系 (多くの場合、ローカル座標系または子座標系と呼ばれる) をもっていると考えることができます。この子座標系は、親座標系を基準にしてオブジェクトとともに回転します。並進がない場合、両方の座標系の原点は一致します。

計算される方向量は、親基準座標系から子基準座標系へ量を変換する回転を表します。この回転は、四元数または回転行列で表されます。

センサーの種類

方向の推定には、加速度計、ジャイロスコープ、磁力計という 3 種類のセンサーが一般的に使用されます。加速度計は固有加速度を測定します。ジャイロスコープは角速度を測定します。磁力計は局所的な磁場を測定します。方向を推定するために、異なるセンサーの組み合わせを融合するさまざまなアルゴリズムが使用されます。

センサー データ

この例の大部分では、同じセンサー データのセットを使用します。加速度計、ジャイロスコープ、および磁力計のセンサー データは、3 つの異なる軸、すなわち最初にローカル "Y" 軸を中心に、次に "Z" 軸を中心に、最後に "X" 軸を中心にデバイスを回転させながら記録されました。実験の間、デバイスの "X" 軸は概ね南を指していました。

ld = load("rpy_9axis.mat");

acc = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;
mag = ld.sensorData.MagneticField;

pp = poseplot;

Figure contains an axes object. The axes object is empty.

加速度計と磁力計の融合

ecompass 関数は、加速度計と磁力計のデータを融合します。これは無記憶アルゴリズムであり、パラメーターを調整する必要はありませんが、センサー ノイズの影響を非常に受けやすいアルゴリズムです。 

qe = ecompass(acc, mag);
for ii=1:size(acc,1)
    set(pp,"Orientation",qe(ii))
    drawnow limitrate
end

Figure contains an axes object. The axes object is empty.

ecompass アルゴリズムは北の位置を正確に特定することに注意してください。ただし、この関数は状態を保持しないため、推定される運動は滑らかにはなりません。このアルゴリズムは、方向フィルターの初期化ステップとして使用したり、Lowpass Filter Orientation Using Quaternion SLERP (Sensor Fusion and Tracking Toolbox)で紹介されているいくつかの手法を使用して運動を滑らかにしたりできます。

加速度計とジャイロスコープの融合

以下のオブジェクトは、誤差状態カルマン フィルターまたは相補フィルターのいずれかを使用して方向を推定します。誤差状態カルマン フィルターは標準的な推定フィルターであり、対応するノイズ パラメーターを使用してシステムのさまざまな側面を調整できます。相補フィルターは、メモリ制約のあるシステムの代替として使用できます。調整可能なパラメーターが最小限であるため設定は容易ですが、詳細な調整はできません。

imufilter および complementaryFilter の各 System object は、加速度計とジャイロスコープのデータを融合します。imufilter は内部で誤差状態カルマン フィルターを使用し、complementaryFilter は相補フィルターを使用します。これらのフィルターは、時間の経過とともにドリフトするジャイロスコープのバイアス ノイズを除去できます。 

ifilt = imufilter(SampleRate=ld.Fs);
for ii=1:size(acc,1)
    qimu = ifilt(acc(ii,:),gyro(ii,:));
    set(pp,"Orientation",qimu)
    drawnow limitrate
end

Figure contains an axes object. The axes object is empty.

% Disable magnetometer input.
cfilt = complementaryFilter(SampleRate=ld.Fs,HasMagnetometer=false);
for ii=1:size(acc,1)
    qimu = cfilt(acc(ii,:),gyro(ii,:));
    set(pp,"Orientation",qimu)
    drawnow limitrate
end

Figure contains an axes object. The axes object is empty.

imufilter および complementaryFilter の各アルゴリズムは、ecompass と比較して運動の推定値を大幅に滑らかにしますが、北の方向を正しく推定しません。imufilter は磁力計データを処理しないため、デバイスの X 軸が初期状態では北を指していると単純に仮定します。imufilter によって得られる運動の推定値は、初期の推定方向を基準としています。complementaryFilter も、HasMagnetometer プロパティを false に設定した場合、同様の仮定を行います。

加速度計、ジャイロスコープ、磁力計の融合

姿勢および進行方位の基準システム (AHRS) は、加速度計、ジャイロスコープ、および磁力計を使用して方向を計算する 9 軸システムで構成されています。ahrsfilter および complementaryFilter の各 System object は、これまでのアルゴリズムの利点を組み合わせることで、デバイスの方向を滑らかに変化する推定値として生成するとともに、北の方向を正しく推定します。complementaryFilter は、前述と同じ相補フィルター アルゴリズムを使用しますが、磁力計を組み込んで方向推定を改善するための追加ステップが含まれています。ahrsfilter アルゴリズムも、imufilter と同様に誤差状態カルマン フィルターを使用します。ahrsfilter は、ジャイロスコープのバイアスの除去に加えて、軽度の磁気妨害を検出して除去する機能も備えています。

ifilt = ahrsfilter(SampleRate=ld.Fs);
for ii=1:size(acc,1)
    qahrs = ifilt(acc(ii,:),gyro(ii,:),mag(ii,:));
    set(pp,"Orientation",qahrs)
    drawnow limitrate
end

Figure contains an axes object. The axes object is empty.

cfilt = complementaryFilter(SampleRate=ld.Fs);
for ii=1:size(acc,1)
    qahrs = cfilt(acc(ii,:),gyro(ii,:),mag(ii,:));
    set(pp,"Orientation",qahrs)
    drawnow limitrate
end

Figure contains an axes object. The axes object is empty.

フィルター パラメーターの調整

complementaryFilter、imufilter、および ahrsfilter の全 System object には、調整可能なパラメーターがあります。使用するセンサーに応じてパラメーターを調整することで、パフォーマンスを向上させることができます。

complementaryFilter パラメーターである AccelerometerGain および MagnetometerGain は、各センサーの測定値が方向の推定に与える影響の大きさを変更するために調整できます。AccelerometerGain を 0 に設定した場合、x 軸および y 軸の方向にはジャイロスコープのみが使用されます。AccelerometerGain を 1 に設定した場合、x 軸および y 軸の方向には加速度計のみが使用されます。MagnetometerGain を 0 に設定した場合、z 軸の方向にはジャイロスコープのみが使用されます。MagnetometerGain を 1 に設定した場合、z 軸の方向には磁力計のみが使用されます。

ahrsfilter および imufilter の各 System object には、特定のハードウェア センサーにフィルターをより厳密に適合させるためのパラメーターがさらに用意されています。センサーの環境も考慮に入れることが重要です。imufilter パラメーターは、ahrsfilter パラメーターのサブセットです。AccelerometerNoise、GyroscopeNoise、MagnetometerNoise、および GyroscopeDriftNoise は、測定ノイズです。センサーのデータシートは、それらの値を決定するのに役立ちます。

LinearAccelerationNoise と LinearAccelerationDecayFactor は、線形 (並進) 加速度に対するフィルターの応答を制御します。デバイスを振ることは、線形加速度を加える簡単な例です。

LinearAccelerationNoise を 9e-3 (m/s2)2 に設定した imufilter が振動軌跡に対してどのように応答するかを、LinearAccelerationNoise を 9e-4 (m/s2)2 に設定したものと比較して考えてみましょう。

ld = load("shakingDevice.mat");
accel = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;

highVarFilt = imufilter(SampleRate=ld.Fs, ...
    LinearAccelerationNoise=0.009);
qHighLANoise = highVarFilt(accel,gyro);

lowVarFilt = imufilter(SampleRate=ld.Fs, ...
    LinearAccelerationNoise=0.0009);
qLowLANoise = lowVarFilt(accel,gyro);

LinearAccelerationNoise の効果を確認する 1 つの方法は、出力の重力ベクトルを見ることです。重力ベクトルは、方向の回転行列の 3 番目の列です。 

rmatHigh = rotmat(qHighLANoise,"frame");
rmatLow = rotmat(qLowLANoise,"frame");

gravDistHigh = sqrt(sum((rmatHigh(:,3,:)-[0;0;1]).^2, 1));
gravDistLow = sqrt(sum((rmatLow(:,3,:)-[0;0;1]).^2, 1));

figure;
plot([squeeze(gravDistHigh),squeeze(gravDistLow)]);
title("Euclidean Distance to Gravity");
legend("LinearAccelerationNoise = 0.009", ...
    "LinearAccelerationNoise = 0.0009");

Figure contains an axes object. The axes object with title Euclidean Distance to Gravity contains 2 objects of type line. These objects represent LinearAccelerationNoise = 0.009, LinearAccelerationNoise = 0.0009.

lowVarFilt は LinearAccelerationNoise が小さいため、線形加速度が小さい環境にあることを想定しています。したがって、プロットの前半の大きな変動が示すように、線形加速度の影響を受けやすくなります。ただし、線形加速度が小さい環境にあることを想定しているため、加速度計の信号に対してより大きな信頼が置かれます。そのため、振動が収まると、方向の推定はすぐに鉛直に収束します。highVarFilt の場合は逆になります。フィルターは振動の影響を受けにくいですが、振動が収まった後に方向の推定が鉛直に収束するまでに時間がかかります。

MagneticDisturbanceNoise プロパティを使用すると、LinearAccelerationNoise が線形加速度をモデル化するのとほぼ同じ方法で、磁気外乱 (地磁気以外のノイズ源) をモデル化できます。

2 つの減衰係数プロパティ (MagneticDisturbanceDecayFactor および LinearAccelerationDecayFactor) は、ノイズの変動率をモデル化します。ゆっくり変動するノイズ源の場合、これらのパラメーターを 1 に近い値に設定します。急速に変動する無相関なノイズの場合、これらのパラメーターを 0 に近い値に設定します。LinearAccelerationDecayFactor を小さくすると、方向の推定で "下" をより早く求められます。MagneticDisturbanceDecayFactor を小さくすると、方向の推定で北をより早く求められます。

非常に大きく短時間の磁気外乱は、ahrsfilter によってほぼ完全に除去されます。固定センサーでの記録中に [0 250 0] uT のパルスが加えられる場合を考えてみましょう。理想的には、方向の推定に変化は生じないはずです。

ld = load("magJamming.mat");
hpulse = ahrsfilter(SampleRate=ld.Fs);
len = 1:10000;
qpulse = hpulse(ld.sensorData.Acceleration(len,:), ...
    ld.sensorData.AngularVelocity(len,:), ...
    ld.sensorData.MagneticField(len,:));

figure;
timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1);
plot( timevec, eulerd(qpulse,"ZYX","frame") );
title(["Stationary Trajectory Orientation Euler Angles" newline ...
    "Magnetic Jamming Response"]);
legend("Z-rotation","Y-rotation","X-rotation");
ylabel("Degrees");
xlabel("Seconds");

Figure contains an axes object. The axes object with xlabel Seconds, ylabel Degrees contains 3 objects of type line. These objects represent Z-rotation, Y-rotation, X-rotation.

この磁気パルスは、フィルターによって干渉としてほぼ完全に除去されることに注意してください。ExpectedMagneticFieldStrength の 4 倍を超える磁場強度は妨害源と見なされ、該当するサンプルにおける磁力計の信号は無視されます。

まとめ

ここで紹介したアルゴリズムは、適切に調整することで、環境ノイズ源に対してロバストな方向の推定を可能にします。センサーが使用される状況を考慮し、それに応じてフィルターを調整することが重要です。