Оцените ориентацию с дополнительным фильтром и данными 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.

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

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 и обсудил эффекты настройки дополнительных параметров фильтра.

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