Оцените ориентацию с дополнительным фильтром и данными IMU

В этом примере показано, как передать данные IMU потоком из Arduino и оценочной ориентации с помощью дополнительного фильтра.

Соедините оборудование

Соедините SDA, SCL, GND и контакты VCC датчика MPU-9250 к соответствующим контактам оборудования Arduino®. Этот пример использует плату Arduino® Uno со следующими связями:

  • SDA - A4

  • SCL - A5

  • VCC - +3.3V

  • GND - GND

Убедитесь, что связи с датчиками неповреждены. Рекомендуется присоединить/соединить датчик к прототипному щиту, чтобы избежать свободных связей, в то время как датчик находится в движении. Пошлите страницу Troubleshooting Sensors отладить связанные с датчиком проблемы.

Создайте объект датчика

Создайте arduino возразите и mpu9250 объект. Задайте частоту дискретизации датчика Fs и количество времени, чтобы запустить циклы. Опционально, включите isVerbose отметьте, чтобы проверять, превышаются ли какие-либо выборки. Путем отключения useHW отметьте, можно также запустить пример с данными о датчике, сохраненными в MAT-файле loggedMPU9250Data.mat.

Данные в loggedMPU9250Data.mat регистрировался, в то время как IMU обычно стоял перед должным Югом, затем вращаемым:

  • +90 градусов вокруг оси z

  • - 180 градусов вокруг оси z

  • +90 градусов вокруг оси z

  • +90 градусов вокруг оси Y

  • - 180 градусов вокруг оси Y

  • +90 градусов вокруг оси Y

  • +90 градусов вокруг оси X

  • - 270 градусов вокруг оси X

  • +180 градусов вокруг оси X

Заметьте, что последние два вращения вокруг оси X являются еще 90 градусами. Это было сделано, чтобы инвертировать устройство вверх тормашками. Итоговая ориентация IMU совпадает с начальной ориентацией, должным Югом.

Fs = 100;
samplesPerRead = 10;
runTime = 20;
isVerbose = false;
useHW = true;

if useHW
    a = arduino;
    imu = mpu9250(a, 'SampleRate', Fs, 'OutputFormat', 'matrix', ...
        'SamplesPerRead', samplesPerRead, 'Streaming', true);
else
    load('loggedMPU9250Data.mat', 'allAccel', 'allGyro', 'allMag', ...
        'allT', 'allOverrun', ...
        'numSamplesAccelGyro', 'numSamplesAccelGyroMag')
end

Выровняйте оси датчика MPU-9250 с координатами NED

Оси акселерометра, гироскопа и магнитометра в MPU-9250 не выравниваются друг с другом. Задайте индекс и подпишите x-, y-, и ось z каждого датчика так, чтобы датчик был выровнен с Северо-востоком вниз (NED) система координат, когда это в покое. В этом примере изменяются оси магнитометра, в то время как акселерометр и оси гироскопа остаются фиксированными. Для ваших собственных приложений измените следующие параметры по мере необходимости.

% Accelerometer axes parameters.
accelXAxisIndex = 1;
accelXAxisSign = 1;
accelYAxisIndex = 2;
accelYAxisSign = 1;
accelZAxisIndex = 3;
accelZAxisSign = 1;

% Gyroscope axes parameters.
gyroXAxisIndex = 1;
gyroXAxisSign = 1;
gyroYAxisIndex = 2;
gyroYAxisSign = 1;
gyroZAxisIndex = 3;
gyroZAxisSign = 1;

% Magnetometer axes parameters.
magXAxisIndex = 2;
magXAxisSign = 1;
magYAxisIndex = 1;
magYAxisSign = 1;
magZAxisIndex = 3;
magZAxisSign = -1;

% Helper functions used to align sensor data axes.

alignAccelAxes = @(in) [accelXAxisSign, accelYAxisSign, accelZAxisSign] ...
    .* in(:, [accelXAxisIndex, accelYAxisIndex, accelZAxisIndex]);

alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign] ...
    .* in(:, [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);

alignMagAxes = @(in) [magXAxisSign, magYAxisSign, magZAxisSign] ...
    .* in(:, [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

Выполните дополнительную калибровку датчика

При необходимости можно калибровать магнитометр, чтобы компенсировать магнитные искажения. Для получения дополнительной информации смотрите раздел Compensating for Hard Iron Distortions Оценки Ориентации Используя Инерционное Cочетание датчиков и MPU-9250 (Sensor Fusion and Tracking Toolbox) пример.

Задайте Дополнительные Параметры фильтра

complementaryFilter имеет два настраиваемых параметра. AccelerometerGain параметр определяет, насколько измерению акселерометра доверяют по измерению гироскопа. MagnetometerGain параметр определяет, насколько измерению магнитометра доверяют по измерению гироскопа.

compFilt = complementaryFilter('SampleRate', Fs)
compFilt = 

  complementaryFilter with properties:

           SampleRate: 100
    AccelerometerGain: 0.0100
     MagnetometerGain: 0.0100
      HasMagnetometer: 1
    OrientationFormat: 'quaternion'

Оцените ориентацию с акселерометром и гироскопом

Установите HasMagnetometer свойство к false отключить вход измерения магнитометра. В этом режиме фильтр только берет акселерометр и измерения гироскопа как входные параметры. Кроме того, фильтр принимает, что начальная ориентация IMU выравнивается с родительской системой координат навигации. Если IMU не будет выровнен с системой координат навигации первоначально, по оценке ориентации будет постоянное смещение.

compFilt = complementaryFilter('HasMagnetometer', false);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
else
    idx = 1:samplesPerRead;
    overrunIdx = 1;
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyro
            break;
        end
    end
end

Оцените ориентацию с акселерометром, гироскопом и магнитометром

Со значениями по умолчанию AccelerometerGain и MagnetometerGain, фильтр доверяет больше на измерениях гироскопа в ближайшей перспективе, но доверяет больше на измерениях акселерометра и магнитометра в долгосрочной перспективе. Это позволяет фильтру быть более реактивным к быстрой ориентации, изменяет и препятствует тому, чтобы оценки ориентации дрейфовали за более длительные промежутки времени. Для определенных датчиков IMU и целей приложения, можно хотеть настроить параметры фильтра, чтобы улучшить точность оценки ориентации.

compFilt = complementaryFilter('SampleRate', Fs);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
        mag = alignMagAxes(mag);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro, mag);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyroMag
            break;
        end
    end
end

Сводные данные

Этот пример показал, как оценить ориентацию IMU использование данных из Arduino и дополнительного фильтра. Этот пример также показал, как сконфигурировать IMU и обсудил эффекты настройки дополнительных параметров фильтра.

Для просмотра документации необходимо авторизоваться на сайте