Оценка ориентации Используя инерционный Fusion датчика и MPU-9250

В этом примере показано, как получить данные из датчика InvenSense MPU-9250 IMU и использовать алгоритмы сплава с 9 осями и с 6 осями в данных о датчике, чтобы вычислить ориентацию устройства.

MPU-9250 является датчиком с 9 осями с акселерометром, гироскопом и магнитометром. Ускорение мер по акселерометру, скорость вращения мер по гироскопу и магнитометр измеряют магнитное поле в x-, y-и z-оси. Ось датчика зависит от того, чтобы делать из датчика.

Необходимый MathWorks® Products

  • MATLAB®

  • Пакет поддержки MATLAB для оборудования Arduino®

  • Sensor Fusion and Tracking Toolbox™

Необходимое оборудование

  • Uno Arduino

  • InvenSense MPU-9250

Аппаратная связь

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

  • SDA - A4

  • SCL - A5

  • VCC - +3.3V

  • GND - GND

Убедитесь, что связи с датчиками неповреждены. Рекомендуется использовать прототипный щит и спаять датчик к нему, чтобы избежать свободных связей при перемещении датчика. Пошлите Датчики Поиска и устранения проблем (Пакет Поддержки MATLAB для Оборудования Arduino) страница для датчиков отладить связанные с датчиком проблемы.

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

Создайте arduino возразите и включайте библиотеку I2C.

a = arduino('COM9', 'Uno', 'Libraries', 'I2C');
Updating server code on board Uno (COM9). This may take a few minutes.

Создайте объект датчика MPU-9250.

fs = 100; % Sample Rate in Hz   
imu = mpu9250(a,'SampleRate',fs,'OutputFormat','matrix'); 

Компенсация трудных железных искажений

Алгоритмы Fusion используют показания магнитометра, которые должны компенсировать магнитные искажения, такие как трудное железное искажение. Трудные железные искажения производятся материалами, которые создают магнитное поле, приводящее к сдвигу источника на поверхности ответа. Эти искажения могут быть откорректированы путем вычитания значений коррекции из показаний магнитометра для каждой оси. Для того, чтобы найти значения коррекции, сделайте следующее:

1. Вращайте датчик от 0 до 360 степеней вокруг каждой оси.

2. Получите минимальные и максимальные показания магнитометра.

3. Насчитайте минимальные и максимальные показания, чтобы получить значения коррекции для каждой оси.

Значения коррекции изменяются со средой.

Следующие фрагменты кода могут использоваться, чтобы получить значения смещения для оси X, подобная процедура может быть выполнена для других осей также:

displayMessage(['Fusion algorithms use magnetometer readings which need to compensate for magnetic distortions.' ...
    'The given code snippet can be used to find the correction values for compensating Hard Iron Distortions. When the code '...
    'is executing, rotate the sensor around x axis from 0 to 360 degree. For other axes, modify the code accordingly and rotate the ' ...
    'sensor along that axis'],'Compensating Hard Iron Distortions');
tic;
stopTimer = 100;
magReadings=[];
while(toc<stopTimer)
    % Rotate the sensor around x axis from 0 to 360 degree.
    % Take 2-3 rotations to improve accuracy.
    % For other axes, rotate around that axis.
    [accel,gyro,mag] = read(imu);
    magReadings = [magReadings;mag];
end

% For y axis, use magReadings (:,2) and for z axis use magReadings(:,3)
magx_min = min(magReadings(:,1));
magx_max = max(magReadings(:,1));
magx_correction = (magx_max+magx_min)/2;

Для более точного отслеживания калибруйте магнитометр для других искажений также. magcal функция (эта функция доступна в Sensor Fusion and Tracking Toolbox™) может использоваться, чтобы компенсировать мягкие железные искажения также. Измените значения коррекции, вычисленные для вашего датчика в readSensorDataMPU9250 функция в примере folder.

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

Алгоритмы Fusion датчика, используемые в этом примере, используют Северо-восток вниз (NED) в качестве фиксированной системы координаты вышестоящего элемента. В системе координат NED Ось X указывает север, Ось Y указывает восток и точки оси Z вниз. В зависимости от алгоритма север может или быть магнитным северным или истинным севером. Алгоритмы в этом примере используют магнитный север. Алгоритмы, используемые здесь, ожидают все датчики в объекте выровнять их ось и соответствуют соглашению NED.

MPU-9250 имеет два устройства, магнитометр и гироскоп акселерометра, на той же плате. Оси этих устройств отличаются друг от друга. Ось магнитометра выравнивается с координатами NED. Ось гироскопа акселерометра отличается от магнитометра в MPU-9250. Акселерометр и ось гироскопа должны быть подкачаны и/или инвертированы, чтобы совпадать с осью магнитометра. Для получения дополнительной информации обратитесь к разделу раздела “Orientation of Axes” в таблице данных MPU-9250.

Чтобы выровнять оси гироскопа акселерометра MPU-9250 к координатам NED, сделайте следующее:

1. Задайте оси устройства: Задайте мнимую ось как ось устройства на датчике в соответствии с системой координат NED, которая может или не может быть тем же самым как оси датчика. Для MPU-9250 ось магнитометра может быть рассмотрена как ось устройства.

2. Подкачайте значения X и Y акселерометра и показаний гироскопа, так, чтобы ось акселерометра и гироскопа была выровнена с осью магнитометра.

3. Определите значения полярности для акселерометра и гироскоп.

a. Акселерометр

  • Поместите датчик, таким образом, что ось X устройства указывает вниз, перпендикуляр на поверхность, в которой сохранен датчик. Показания акселерометра должны читать приблизительно [9.8 0 0]. Если не инвертируют x-значения акселерометра.

  • Поместите датчик, таким образом, что ось Y устройства указывает вниз, перпендикуляр на поверхность, в которой сохранен датчик. Показания акселерометра должны читать приблизительно [0 9.8 0]. Если не инвертируют y-значения акселерометра.

  • Поместите датчик, таким образом, что ось Z устройства указывает вниз, перпендикуляр на поверхность, в которой сохранен датчик. Показания акселерометра должны читать приблизительно [0 0 9.8]. Если не инвертируют z-значения акселерометра.

b. Гироскоп

Вращайте датчик вдоль каждой оси и получите показания. Используйте правое правило винта, чтобы откорректировать полярность вращения.

Вышеупомянутый метод используется, чтобы установить ось датчика в этом примере.

Настройка параметров фильтра

Алгоритмы, используемые в этом примере, когда правильно настроено, включают оценку ориентации и устойчивы против экологических источников шума. Необходимо рассмотреть ситуации, в которых датчики используются и настраивают фильтры соответственно. Отошлите Настраивающиеся Параметры Фильтра для получения дополнительной информации, связанных с настраивающимися параметрами фильтра.

Пример демонстрирует три алгоритма, чтобы определить ориентацию, а именно, ahrsfilter, imufilter, и ecompass. Относитесь Определяют Ориентацию Используя Инерционные Датчики для получения дополнительной информации, связанных с инерционными алгоритмами сплава.

Fusion магнитометра гироскопа акселерометра

И заголовок ссылочной системы (AHRS) отношения состоит из 9 систем координат, которые используют акселерометр, гироскоп и магнитометр, чтобы вычислить ориентацию устройства. ahrsfilter производит гладко изменяющуюся оценку ориентации устройства, правильно оценивая северное направление. ahrsfilter имеет способность удалить смещение гироскопа и может также обнаружить и отклонить умеренный магнитный затор.

Следующие фрагменты кода используют ahrsfilter системный объект, чтобы определить ориентацию датчика и создает фигуру, которая обновляется, когда вы перемещаете датчик. Датчик должен быть стационарным перед запуском этого примера.

% GyroscopeNoise and AccelerometerNoise is determined from datasheet.
GyroscopeNoiseMPU9250 = 3.0462e-06; % GyroscopeNoise (variance value) in units of rad/s
AccelerometerNoiseMPU9250 = 0.0061; % AccelerometerNoise(variance value)in units of m/s^2
viewer = HelperOrientationViewer('Title',{'AHRS Filter'});
FUSE = ahrsfilter('SampleRate',imu.SampleRate, 'GyroscopeNoise',GyroscopeNoiseMPU9250,'AccelerometerNoise',AccelerometerNoiseMPU9250);
stopTimer = 100;

В то время как ниже кода становится выполняемым, медленно перемещайте датчик и проверку, если движение в фигуре совпадает с движением датчика.

% Use ahrsfilter to estimate orientation and update the viewer as the
% sensor moves for time specified by stopTimer
displayMessage(['This section uses AHRS filter to determine orientation of the sensor by collecting live sensor data from the \slmpu9250 \rm' ...
    'system object. Move the sensor to visualize orientation of the sensor in the figure window. Keep the sensor stationery before you' ...
    'click OK'],...
    'Estimate Orientation using AHRS filter and MPU-9250')
tic;
while(toc < stopTimer)
    [accel,gyro,mag] = readSensorDataMPU9250(imu);
    rotators = FUSE(accel,gyro,mag);
    for j = numel(rotators)
        viewer(rotators(j));
    end
end

Когда ось X устройства датчика указывает на север, Ось Y устройства указывает на восток, и ось Z устройства указывает вниз.

Когда ось X устройства датчика указывает на север, Ось Y устройства указывает на запад, и ось Z устройства указывает вверх.

Fusion гироскопа акселерометра

imufilter системный объект плавит акселерометр и данные о гироскопе с помощью внутреннего Фильтра Калмана состояния ошибки. Фильтр способен к удалению шума смещения гироскопа, который дрейфует в зависимости от времени. Фильтр не обрабатывает данные о магнитометре, таким образом, это правильно не оценивает направление севера. Алгоритм принимает, что исходное положение датчика находится таким путем, которым Ось X устройства датчика указывает к магнитному северу, Ось Y устройства датчика указывает на восток, и ось Z устройства датчика указывает вниз. Датчик должен быть стационарным перед запуском этого примера.

Следующие фрагменты кода используют imufilter объект определить ориентацию датчика и создает фигуру, которая обновляется, когда вы перемещаете датчик.

displayMessage(['This section uses  IMU filter to determine orientation of the sensor by collecting live sensor data from the \slmpu9250 \rm' ...
    'system object. Move the sensor to visualize orientation of the sensor in the figure window. Keep the sensor stationery before you'...
    'click OK'],...
    'Estimate Orientation using IMU filter and MPU-9250.')
% GyroscopeNoise and AccelerometerNoise is determined from datasheet.
GyroscopeNoiseMPU9250 = 3.0462e-06; % GyroscopeNoise (variance) in units of rad/s
AccelerometerNoiseMPU9250 = 0.0061; % AccelerometerNoise (variance) in units of m/s^2
viewer = HelperOrientationViewer('Title',{'IMU Filter'});
FUSE = imufilter('SampleRate',imu.SampleRate, 'GyroscopeNoise',GyroscopeNoiseMPU9250,'AccelerometerNoise', AccelerometerNoiseMPU9250);
stopTimer=100;

В то время как ниже кода становится выполняемым, медленно перемещайте датчик и проверку, если движение в фигуре совпадает с движением датчика.

% Use imufilter to estimate orientation and update the viewer as the
% sensor moves for time specified by stopTimer
tic;
while(toc < stopTimer)
    [accel,gyro] = readSensorDataMPU9250(imu);
    rotators = FUSE(accel,gyro);
    for j = numel(rotators)
        viewer(rotators(j));
    end
end

imufilter алгоритм может также использоваться с MPU6050 также, поскольку это не требует значений магнитометра.

Когда ось X устройства датчика указывает на север, ось Z устройства указывает вниз, и Ось Y устройства указывает на восток.

Когда ось X устройства датчика указывает вверх, Ось Y устройства является точками на запад, и ось Z устройства указывает на юг.

Fusion магнитометра акселерометра

ecompass системный объект объединяет данные об акселерометре и магнитометре. Алгоритм Ecompass является алгоритмом без памяти, который не требует никакой настройки параметра, но очень восприимчив к шуму датчика. Вы могли использовать сферическую линейную интерполяцию (SLERP) для фильтра lowpass шумная траектория. Отошлите Ориентацию Фильтра Lowpass с помощью Кватерниона пример SLERP в большем количестве деталей

displayMessage(['This section uses \slecompass \rmfunction to determine orientation of the sensor by collecting live sensor data from the \slmpu9250 ' ...
    '\rmsystem object. Move the sensor to visualize orientation of the sensor in the figure window. Keep the sensor stationery before you click OK'],...
    'Estimate Orientation using Ecompass algorithm.')
tic;
viewer = HelperOrientationViewer('Title',{'Ecompass Algorithm'});
stopTimer = 100;
tic;
% Use ecompass algorithm to estimate orientation and update the viewer as the
% sensor moves for time specified by stopTimer.
while(toc < stopTimer)
    [accel,gyro,mag] = readSensorDataMPU9250(imu);
    rotators = ecompass(accel,mag);
    for j = numel(rotators)
        viewer(rotators(j));
    end
end

Очистка

Когда связь больше не будет необходима, выпустите и очистите объекты

release(imu);
delete(imu);
clear;

Вещи попробовать

Можно попробовать этот пример другими датчиками, такими как InvenSense MPU-6050 и STMicroelectronics LSM9DS1. Обратите внимание на то, что датчик MPU-6050 может использоваться только с imufilter системный объект.

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