Необходимые продукты MathWorks ®

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

Аппаратное подключение

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

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

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

Создание 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'); 

Компенсация искажений твердого железа

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

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 функция (эта функция доступна как в Toolbox™ Sensor Fusion and Tracking, так и в навигационном Toolbox™) также может использоваться для компенсации искажений мягкого железа. Измените значения коррекции, рассчитанные для датчика в readSensorDataMPU9250 функция в папке-примере.

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

Алгоритмы слияния датчиков, используемые в этом примере, используют NED (North-East-Down) в качестве фиксированной родительской системы координат. В системе координат NED ось X указывает на север, ось Y - на восток, а ось Z - вниз. В зависимости от алгоритма, север может быть либо магнитным севером, либо истинным севером. Алгоритмы в этом примере используют магнитный север. Алгоритмы, используемые здесь, ожидают, что все датчики в объекте выровнены и соответствуют конвенции NED .

MPU-9250 имеет два устройства, магнитометр и акселерометр-гироскоп, на одной плате. Оси этих устройств отличаются друг от друга. Ось магнитометра совмещена с координатами NED. Ось акселерометра-гироскопа отличается от магнитометра в MPU-9250. Акселерометр и ось гироскопа необходимо поменять местами и/или перевернуть для соответствия оси магнитометра. Для получения дополнительной информации см. раздел «Ориентация осей» MPU-9250 спецификации.

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

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

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

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

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

b. Гироскоп

Поверните датчик вдоль каждой оси и зафиксируйте показания. Для корректировки полярности вращения используйте правило правого винта.

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

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

Алгоритмы, используемые в этом примере, при правильной настройке позволяют оценивать ориентацию и являются устойчивыми к воздействию источников шума окружающей среды. Необходимо рассмотреть ситуации, в которых используются датчики, и соответствующим образом настроить фильтры. Дополнительные сведения о настройке параметров фильтра см. в разделе «Настройка параметров фильтра» документа «Оценка ориентации по инерционному синтезу датчиков» (Sensor Fusion and Tracking Toolbox).

Пример демонстрирует три алгоритма для определения ориентации, а именно: ahrsfilter, imufilter, и ecompass. Дополнительные сведения о алгоритмах инерционного слияния см. в разделе Определение ориентации с помощью инерционных датчиков (панель инструментов слияния и отслеживания датчиков).

Акселерометр-гироскоп-магнитометрический синтез

Опорная система ориентации и курса (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 устройства - на юг.

Синтез акселерометра-магнитометра

ecompass объект системы плавит данные акселерометра и магнитометра. Алгоритм Ecompass - это алгоритм без памяти, который не требует настройки параметров, но очень чувствителен к шуму датчика. Можно использовать сферическую линейную интерполяцию (SLERP) для фильтрации нижних частот шумной траектории. Дополнительные сведения см. в примере «Ориентация фильтра нижних частот с использованием кватерниона SLERP» (Sensor Fusion and Tracking Toolbox)

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 системный объект.