Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

地上車両の位置と向きの推定

この例では、慣性測定ユニット (IMU) と全地球測位システム (GPS) 受信機からのデータを融合させて、地上車両の位置と向きを推定する方法を説明します。

シミュレーションの設定

サンプリング レートを設定します。典型的なシステムでは、IMU 内の加速度計およびジャイロスコープは、比較的高いサンプル レートで動作します。これらのセンサーからのデータを融合アルゴリズムで処理することは、それほど複雑ではありません。逆に、GPS は比較的低いサンプル レートで動作するため、その処理に関連する複雑度は高くなります。この融合アルゴリズムでは、GPS のサンプルは低いレートで処理され、加速度計とジャイロスコープのサンプルは共に同じ高いレートで処理されます。

この構成をシミュレートするために、IMU (加速度計とジャイロスコープ) は 100 Hz でサンプリングされ、GPS は 10 Hz でサンプリングされます。

imuFs = 100;
gpsFs = 10;

% Define where on the Earth this simulation takes place using latitude,
% longitude, and altitude (LLA) coordinates.
localOrigin = [42.2825 -71.343 53.0352];

% Validate that the |gpsFs| divides |imuFs|. This allows the sensor sample
% rates to be simulated using a nested for loop without complex sample rate
% matching.

imuSamplesPerGPS = (imuFs/gpsFs);
assert(imuSamplesPerGPS == fix(imuSamplesPerGPS), ...
    'GPS sampling rate must be an integer factor of IMU sampling rate.');

融合フィルター

IMU と GPS の測定を融合するフィルターを作成します。融合フィルターは、拡張カルマン フィルターを使用して、向き (四元数として)、位置、速度、およびセンサー バイアスを追跡します。

insfilterNonholonomic オブジェクトには、predictfusegps という 2 つのメイン メソッドがあります。predict メソッドは、IMU から加速度計およびジャイロスコープのサンプルを入力として取ります。加速度計とジャイロスコープがサンプリングされるたびに、predict メソッドを呼び出します。このメソッドは、加速度計とジャイロスコープに基づいて、1 タイム ステップ先の状態を予測します。拡張カルマン フィルターの誤差の共分散は、このステップで更新されます。

fusegps メソッドは、GPS のサンプルを入力として取ります。このメソッドは、さまざまなセンサー入力を不確かさに従って重み付けするカルマン ゲインを計算することにより、GPS サンプルに基づいてフィルターの状態を更新します。誤差の共分散はこのステップでも更新され、今回はカルマン ゲインも使用されます。

insfilterNonholonomic オブジェクトには、IMUSampleRateDecimationFactor という 2 つのメイン プロパティがあります。地上車両には、地面から飛び上がることも地面でスライドすることもないと想定する、2 つの速度制約があります。これらの制約は、拡張カルマン フィルターの更新方程式を使用して適用されます。これらの更新は、IMUSampleRate/DecimationFactor Hz のレートでフィルターの状態に適用されます。

gndFusion = insfilterNonholonomic('ReferenceFrame', 'ENU', ...
    'IMUSampleRate', imuFs, ...
    'ReferenceLocation', localOrigin, ...
    'DecimationFactor', 2);

地上車両の軌跡の作成

waypointTrajectory オブジェクトは、指定されたサンプリング レート、ウェイポイント、到着時間、および向きに基づいて姿勢を計算します。地上車両の円形軌跡のパラメーターを指定します。

% Trajectory parameters
r = 8.42; % (m)
speed = 2.50; % (m/s)
center = [0, 0]; % (m)
initialYaw = 90; % (degrees)
numRevs = 2;

% Define angles theta and corresponding times of arrival t.
revTime = 2*pi*r / speed;
theta = (0:pi/2:2*pi*numRevs).';
t = linspace(0, revTime*numRevs, numel(theta)).';

% Define position.
x = r .* cos(theta) + center(1);
y = r .* sin(theta) + center(2);
z = zeros(size(x));
position = [x, y, z];

% Define orientation.
yaw = theta + deg2rad(initialYaw);
yaw = mod(yaw, 2*pi);
pitch = zeros(size(yaw));
roll = zeros(size(yaw));
orientation = quaternion([yaw, pitch, roll], 'euler', ...
    'ZYX', 'frame');

% Generate trajectory.
groundTruth = waypointTrajectory('SampleRate', imuFs, ...
    'Waypoints', position, ...
    'TimeOfArrival', t, ...
    'Orientation', orientation);

% Initialize the random number generator used to simulate sensor noise.
rng('default');

GPS 受信機

指定のサンプル レートと参照場所で GPS を設定します。他のパラメーターは、出力信号内のノイズの性質を制御します。

gps = gpsSensor('UpdateRate', gpsFs, 'ReferenceFrame', 'ENU');
gps.ReferenceLocation = localOrigin;
gps.DecayFactor = 0.5;                % Random walk noise parameter
gps.HorizontalPositionAccuracy = 1.0;
gps.VerticalPositionAccuracy =  1.0;
gps.VelocityAccuracy = 0.1;

IMU センサー

通常、地上車両は姿勢の推定に 6 軸の IMU センサーを使用します。IMU センサーをモデル化するには、加速度計とジャイロスコープを含む IMU センサー モデルを定義します。実際のアプリケーションでは、2 つのセンサーは、単一の集積回路によることも別々の回路に由来することもあります。ここで設定するプロパティ値は、低コストの MEMS センサーに典型的なものです。

imu = imuSensor('accel-gyro', ...
    'ReferenceFrame', 'ENU', 'SampleRate', imuFs);

% Accelerometer
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.NoiseDensity = 0.0012356;

% Gyroscope
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.NoiseDensity = deg2rad(0.025);

insfilterNonholonomic の状態の初期化

以下の状態があります。

States                            Units    Index
Orientation (quaternion parts)             1:4
Gyroscope Bias (XYZ)              rad/s    5:7
Position (NED)                    m        8:10
Velocity (NED)                    m/s      11:13
Accelerometer Bias (XYZ)          m/s^2    14:16

フィルターが短時間で適切な回答に収束するよう、フィルターの状態の初期化を補助するためにグラウンド トゥルースが使用されます。

% Get the initial ground truth pose from the first sample of the trajectory
% and release the ground truth trajectory to ensure the first sample is not
% skipped during simulation.
[initialPos, initialAtt, initialVel] = groundTruth();
reset(groundTruth);

% Initialize the states of the filter
gndFusion.State(1:4) = compact(initialAtt).';
gndFusion.State(5:7) = imu.Gyroscope.ConstantBias;
gndFusion.State(8:10) = initialPos.';
gndFusion.State(11:13) = initialVel.';
gndFusion.State(14:16) = imu.Accelerometer.ConstantBias;

insfilterNonholonomic の分散の初期化

測定ノイズは、どれだけのノイズが GPS の読み取り値を破損しているかを、gpsSensor パラメーターと車両の動的モデルにある不確かさの程度に基づいて記述します。

プロセス ノイズは、フィルター方程式が状態の進展をどの程度よく説明しているかを記述します。プロセス ノイズは、パラメーターのスイープを使用してフィルターからの位置と向きの推定を一緒に最適化することにより、経験的に決定されます。

% Measurement noises
Rvel = gps.VelocityAccuracy.^2;
Rpos = gps.HorizontalPositionAccuracy.^2;

% The dynamic model of the ground vehicle for this filter assumes there is
% no side slip or skid during movement. This means that the velocity is
% constrained to only the forward body axis. The other two velocity axis
% readings are corrected with a zero measurement weighted by the
% |ZeroVelocityConstraintNoise| parameter.
gndFusion.ZeroVelocityConstraintNoise = 1e-2;

% Process noises
gndFusion.GyroscopeNoise = 4e-6;
gndFusion.GyroscopeBiasNoise = 4e-14;
gndFusion.AccelerometerNoise = 4.8e-2;
gndFusion.AccelerometerBiasNoise = 4e-14;

% Initial error covariance
gndFusion.StateCovariance = 1e-9*ones(16);

スコープの初期化

HelperScrollingPlotter スコープを使うと、変数の経時的なプロットが可能になります。ここでは、姿勢の誤差を追跡するために使用します。HelperPoseViewer スコープを使うと、フィルター推定値とグラウンド トゥルース姿勢を 3-D で可視化できます。スコープによってシミュレーション速度は低下する場合があります。スコープを無効にするには、対応する論理変数を false に設定します。

useErrScope = true; % Turn on the streaming error plot
usePoseView = true;  % Turn on the 3D pose viewer

if useErrScope
    errscope = HelperScrollingPlotter( ...
            'NumInputs', 4, ...
            'TimeSpan', 10, ...
            'SampleRate', imuFs, ...
            'YLabel', {'degrees', ...
            'meters', ...
            'meters', ...
            'meters'}, ...
            'Title', {'Quaternion Distance', ...
            'Position X Error', ...
            'Position Y Error', ...
            'Position Z Error'}, ...
            'YLimits', ...
            [-1, 1
             -1, 1
             -1, 1
             -1, 1]);
end

if usePoseView
    viewer = HelperPoseViewer( ...
        'XPositionLimits', [-15, 15], ...
        'YPositionLimits', [-15, 15], ...
        'ZPositionLimits', [-5, 5], ...
        'ReferenceFrame', 'ENU');
end

シミュレーション ループ

メインのシミュレーション ループは、for ループが入れ子になっている while ループです。while ループは、GPS 測定レートである gpsFs で実行されます。入れ子になった for ループは、IMU サンプル レートである imuFs で実行されます。スコープは IMU サンプル レートで更新されます。

totalSimTime = 30; % seconds

% Log data for final metric computation.
numsamples = floor(min(t(end), totalSimTime) * gpsFs);
truePosition = zeros(numsamples,3);
trueOrientation = quaternion.zeros(numsamples,1);
estPosition = zeros(numsamples,3);
estOrientation = quaternion.zeros(numsamples,1);

idx = 0;

for sampleIdx = 1:numsamples
    % Predict loop at IMU update frequency.
    for i = 1:imuSamplesPerGPS
        if ~isDone(groundTruth)
            idx = idx + 1;

            % Simulate the IMU data from the current pose.
            [truePosition(idx,:), trueOrientation(idx,:), ...
                trueVel, trueAcc, trueAngVel] = groundTruth();
            [accelData, gyroData] = imu(trueAcc, trueAngVel, ...
                trueOrientation(idx,:));

            % Use the predict method to estimate the filter state based
            % on the accelData and gyroData arrays.
            predict(gndFusion, accelData, gyroData);

            % Log the estimated orientation and position.
            [estPosition(idx,:), estOrientation(idx,:)] = pose(gndFusion);

            % Compute the errors and plot.
            if useErrScope
                orientErr = rad2deg( ...
                    dist(estOrientation(idx,:), trueOrientation(idx,:)));
                posErr = estPosition(idx,:) - truePosition(idx,:);
                errscope(orientErr, posErr(1), posErr(2), posErr(3));
            end

            % Update the pose viewer.
            if usePoseView
                viewer(estPosition(idx,:), estOrientation(idx,:), ...
                    truePosition(idx,:), estOrientation(idx,:));
            end
        end
    end

    if ~isDone(groundTruth)
        % This next step happens at the GPS sample rate.
        % Simulate the GPS output based on the current pose.
        [lla, gpsVel] = gps(truePosition(idx,:), trueVel);

        % Update the filter states based on the GPS data.
        fusegps(gndFusion, lla, Rpos, gpsVel, Rvel);
    end
end

誤差メトリクスの計算

位置と向きは、シミュレーション全体を通してログに記録されました。ここで、位置と向きの両方についてエンドツーエンドの平方根平均二乗誤差を計算します。

posd = estPosition - truePosition;

% For orientation, quaternion distance is a much better alternative to
% subtracting Euler angles, which have discontinuities. The quaternion
% distance can be computed with the |dist| function, which gives the
% angular difference in orientation in radians. Convert to degrees for
% display in the command window.

quatd = rad2deg(dist(estOrientation, trueOrientation));

% Display RMS errors in the command window.
fprintf('\n\nEnd-to-End Simulation Position RMS Error\n');
msep = sqrt(mean(posd.^2));
fprintf('\tX: %.2f , Y: %.2f, Z: %.2f   (meters)\n\n', msep(1), ...
    msep(2), msep(3));

fprintf('End-to-End Quaternion Distance RMS Error (degrees) \n');
fprintf('\t%.2f (degrees)\n\n', sqrt(mean(quatd.^2)));

End-to-End Simulation Position RMS Error
	X: 1.16 , Y: 0.99, Z: 0.03   (meters)

End-to-End Quaternion Distance RMS Error (degrees) 
	0.09 (degrees)