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

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

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

Подключите контакты SDA, SCL, GND и VCC датчика BNO055 к соответствующим контактам платы Arduino ® Uno с помощью соединений:

Подключите контакты TX, RX, GND и VCC модуля HC-05 к соответствующим контактам платы Arduino ® Uno. В этом примере используются соединения:

Убедитесь, что соединения с датчиками и модулем Bluetooth ® не повреждены. Рекомендуется использовать BNO055 щит для Arduino Uno (Arduino 9 Axis Motion Shield). См. раздел Устранение неполадок датчиков (пакет поддержки MATLAB для оборудования Arduino) для отладки проблем, связанных с датчиками.

Настройка и настройка Arduino для Bluetooth ® Communication

Настройте плату Arduino Uno для взаимодействия через Bluetooth ® с помощью arduinosetup из командной строки MATLAB. Инструкции по настройке платы Arduino для связи через Bluetooth ® см. в разделе Установка и настройка оборудования Arduino. Убедитесь, что установлен флажок «Библиотеки I2C» для включения в программу установки .

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

Создание arduino объект.

a = arduino('btspp://98D33230EB9F', 'Uno');

Создать bno055 объект датчика в OperatingMode 'amg'.

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

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

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

Эти корректирующие значения изменяются в зависимости от окружающей среды.

Для получения поправочных коэффициентов как для искажения твердого железа, так и для искажения мягкого железа:

ts = tic; 
stopTimer = 50; 
magReadings=[]; 
while(toc(ts) < stopTimer) 
    % Rotate the sensor along x axis from 0 to 360 degree. 
    % Take 2-3 rotations to improve accuracy. 
    % For other axes, rotate along that axes. 
   [accel,gyro,mag] = read(imu); 
   magReadings = [magReadings;mag]; 
end 

[A, b] = magcal(magReadings); % A = 3x3 matrix for soft iron correction 
                              % b = 3x1 vector for hard iron correction 

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

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

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

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

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

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

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

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

% GyroscopeNoise, AccelerometerNoise and MagnetometerNoise are determined from the BNO055 datasheet using the following formula 
% NoisePower = OutputNoisePowerDensityrms^2 * Bandwidth 

GyroscopeNoiseBNO055 = 3.05e-06; % GyroscopeNoise (variance value) in units of (rad/s)^2 
AccelerometerNoiseBNO055 = 67.53e-06; % AccelerometerNoise (variance value)in units of (m/s^2)^2 
MagnetometerNoiseBNO055 = 1; %MagnetometerNoise (variance value) in units of uT^2 

viewer = HelperOrientationViewer('Title',{'AHRS Filter'}); 

FUSE = ahrsfilter('SampleRate',imu.SampleRate,'GyroscopeNoise',GyroscopeNoiseBNO055,'AccelerometerNoise',AccelerometerNoiseBNO055,'MagnetometerNoise',MagnetometerNoiseBNO055); 

stopTimer=10; 

После выполнения приведенного ниже фрагмента кода медленно переместите датчик и проверьте, соответствует ли движение на рисунке движению датчика. Увеличить stopTimer значение, если необходимо отслеживать ориентацию в течение более длительного времени.

magx_correction = b(1); 
magy_correction = b(2); 
magz_correction = b(3); 
 
ts = tic; 
while(toc(ts) < stopTimer) 
    [accel,gyro,mag] = read(imu); 
    % Align coordinates in accordance with NED convention 
    accel = [-accel(:,1), accel(:,2), accel(:,3)]; 
    gyro = [gyro(:,1), -gyro(:,2), -gyro(:,3)]; 
    mag = [(mag(:,1)-magx_correction), -(mag(:,2)- magy_correction), -(mag(:,3)-magz_correction)] * A; 
    rotators = FUSE(accel,gyro,mag); 
    for j = numel(rotators) 
      viewer(rotators(j)); 
    end 
end

Если датчик неподвижен в исходном положении, где ось x устройства указывает на магнитный север, y-ось устройства указывает на восток, а z-ось устройства - вниз, ось X на рисунке будет параллельна положительной оси X и выровнена с ней, ось y на рисунке будет параллельна положительной оси y и выровнена с ней, и ось Z на рисунке будет параллельна положительной оси Z и выровнена с ней.

Очистить

Если подключение больше не требуется, освободите и очистите объекты.

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

Вещи, которые нужно попробовать

Этот пример можно попробовать с другими датчиками, такими как InvenSense MPU-6050, MPU-9250 и STMicroelectronics LSM9DS1.