Подключение оборудования

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

Убедитесь, что соединения с датчиками целы. Рекомендуется прикрепить/подключить датчик к экрану прототипа, чтобы избежать свободных соединений во время движения датчика. Для отладки проблем, связанных с датчиком, см. страницу «Поиск и устранение проблем датчиков».

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

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

Данные в loggedMPU9250Data.mat был записан в журнал, в то время как БИНС, как правило, сталкивался с South, а затем вращался:

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

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-ось каждого датчика так, чтобы датчик был выровнен с системой координат North-East-Down (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]);

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

При необходимости можно калибровать магнитометр, чтобы компенсировать магнитные искажения. Для получения дополнительной информации смотрите раздел «Компенсация искажений твёрдого железа» в Оценке ориентации с использованием синтеза инерционного датчика и MPU-9250 примере.

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

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

compFilt = complementaryFilter('SampleRate', Fs)

compFilt = 

  complementaryFilter with properties:

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

Оценка ориентации с помощью Accelerometer и гироскопа

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

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фильтр больше доверяет измерениям гироскопа в краткосрочной перспективе, но больше доверяет измерениям акселерометра и магнитометра в долгосрочной перспективе. Это позволяет фильтру быть более реактивным к быстрым изменениям ориентации и препятствует дрейфу оценок ориентации в течение более длительных периодов времени. Для конкретных датчиков БИНС и прикладных целей можно захотеть настроить параметры фильтра, чтобы улучшить точность оценки ориентации.

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

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

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