Опыты с акселерометром и гироскопом на платформе S7V30

в 7:22, , рубрики: ISM330, акселерометр, гироскоп
Опыты с акселерометром и гироскопом на платформе S7V30 - 1

Модуль S7V30 изначально разработан для управления промышленными контроллерами, но может быть использован и автономно. В этой статье я попробую раскрыть его потенциал как сенсора для обнаружения и анализа вибраций, углов отклонения, аномальных состояний и специфических активностей. Для этого мне помогут MATLAB и ChatGPT.

Предыдущие статьи по теме модуля S7V30

На плате модуля установлен чип ISM330DLC от STMicroelectronics – iNEMO 6-axis IMU, который совмещает трёхосный акселерометр и гироскоп, обеспечивая цифровой вывод данных для промышленных приложений.

Подключение и расположение

Опыты с акселерометром и гироскопом на платформе S7V30 - 2

При работе с такими чипами важно знать его ориентацию на плате,  от этого зависит корректность определения поворотов и векторов движения.

Опыты с акселерометром и гироскопом на платформе S7V30 - 3
Опыты с акселерометром и гироскопом на платформе S7V30 - 4

Чип подключён по шине I2C с частотой до 400 кГц, и на этой же шине работают чип зарядного устройства и измеритель уровня заряда. Это накладывает ограничения на пропускную способность шины: например, максимальная частота выборок по трём осям гироскопа и акселерометра составляет 1666 Гц – иначе шина просто не справится с потоком данных.

Получение и анализ данных

Для анализа удобнее всего отправлять данные с акселерометра на ПК в режиме реального времени. Модуль поддерживает сбор данных как онлайн, так и оффлайн – они могут сохраняться на внутреннюю SD-карту. Передача данных на компьютер осуществляется через высокоскоростной USB или Wi-Fi, используя протоколы TCP/IP, FTP и FreeMaster. Скачиавем файлы записей по FTP, живые графики и телеметрию модуля смотрим через среду FreeMaster, для глубокого анализа используем TCP подключение прямо в MATLAB.

При разработке нейросетевых алгоритмов значительное ускорение работы даёт автоматическая аннотация данных. Её можно организовать с помощью дополнительных сенсоров, расположенных непосредственно на объектах наблюдения в труднодоступных местах, что особенно удобно благодаря компактности и автономности модуля. К тому же модули можно синхронизировать для совместной работы в группе.

Регистры и возможности чипа

Чип ISM330DLC содержит 93 доступных 8-разрядных регистра. Такое количество обусловлено тем, что чип может самостоятельно распознавать определённые типы событий, такие как свободное падение, наклон, одиночные и двойные клики и другие. Однако в этой статье мы не будем исследовать эти функции, поскольку они связаны с задачами минимизации энергопотребления, а наша цель – точное измерение физических воздействий на чип и его ориентации.

В текущем проекте реализовано терминальное окно, которое позволяет наблюдать и изменять значения всех доступных регистров ISM330DLC. К терминалу можно подключится через клиента Telnet протокола. Например TeraTerm по URL telnet://S7V30

Опыты с акселерометром и гироскопом на платформе S7V30 - 5

По нажатию последовательности кнопок 7 -> 8 -> 4 будет открыто окно с доступом к регистрам:

Опыты с акселерометром и гироскопом на платформе S7V30 - 6

Здесь на экране в реальном времени отображаются данные регистров и есть возможность записать любые произвольные данные в любой регистр.
В нашем проекте уже реализована процедура инициализации чипа в соответствии с пользовательскими настройками. Пользовательские настройки хранятся в энергонезависимой памяти модуля и доступны также по интерфейсу telenet  

Начальный этап: калибровка

Первый шаг – это определение шумовых характеристик и дрейфа сенсора. Это критически важно для точности последующих измерений и анализа данных.

Главное, что необходимо знать — это то, что показания MEMS акселерометров и гироскопов подвержены значительному шуму и накоплению ошибки (дрейфу) с течением времени. .
Поэтому чипы типа ISM330DLC не совсем годятся для прецизионных измерений. Ниже типичный график сигналов в среде FreeMaster принимаемых от ISM330DLC в модуле S7V30, когда он находится в покое. Хотя абсолютный покой в реальном помещении плохо достижим, акселерометры чутко реагируют даже если за окном дует ветер. 

Опыты с акселерометром и гироскопом на платформе S7V30 - 7

Вот график сигналов в течении 6 мин состояния покоя с вычисленными RMS шума и средним значением для каждой оси акселерометра и гироскопа:

Опыты с акселерометром и гироскопом на платформе S7V30 - 8

А здесь график с тем же сигналом, но отфильтрованным бегущим средним с окном в 30 сек. График показывает дрейфы в течении 6 минут

Опыты с акселерометром и гироскопом на платформе S7V30 - 9

Для сравнения посмотрим что нам обещает даташит:

Опыты с акселерометром и гироскопом на платформе S7V30 - 10

Дрейф гироскопа по оси Y чуть больше обещанных 2 dps (градусов в сек). RMS шума гироскопа значительно больше заявленных  75 mdps и доходит до 785 mdps.

RMS шума акселерометра согласно вычислениям на диапазоне 2g должен быть около 7 mg, а по факту доходит до 24 mg.
Но это не научная статья. Просто снимок того что можно получить в гаражных условиях. Видимо помещение несмотря на тишину было довольно шумным.

Как считается RMS шума акселерометра

Для определения RMS шума при частоте сэмплирования 1666 Гц, учитывая, что плотность шума акселерометра составляет 170 , mu g / sqrt{text{Hz}}, можно использовать следующее выражение:

text{RMS}=text{Noise Density} times sqrt{Delta f}

где Delta f — это полоса частот, равная частоте сэмплирования. Подставляем значения:

text{RMS}=170 times 10^{-6} , g / sqrt{text{Hz}} times sqrt{1666 , text{Hz}}

 text{RMS} approx 170 times 10^{-6} times 40.82 approx 6.94 times 10^{-3} , g=6.94 , text{mg}

Таким образом, RMS шума составляет приблизительно 6.94 mg.

Срипт MATLAB для анализа шума акселерометра и гироскопа
% MATLAB скрипт для чтения двоичных данных и построения графиков акселерометра и гироскопа
% с игнорированием первой секунды данных, конвертацией в физические величины и отображением RMS для каждого сигнала

% Константы
fs = 1666; % Частота дискретизации, Гц
G = 9.80665; % Ускорение свободного падения, м/с²
PI = 3.141592653589793;

% Диапазоны акселерометра и гироскопа
ACCEL_RANGE_G = 2;   % ±2g
GYRO_RANGE_DPS = 250; % ±250°/с

% Коэффициенты масштабирования для окончательных величин
acc_g = (ACCEL_RANGE_G / 32768) * G; % м/с²/LSB
gyro_g = (GYRO_RANGE_DPS / 32768); % град/с/LSB

% Запрос файла у пользователя
[filename, pathname] = uigetfile('*.bin', 'Выберите двоичный файл данных');
if isequal(filename, 0)
    errordlg('Файл не выбран. Скрипт завершен.', 'Ошибка');
    return;
end
filepath = fullfile(pathname, filename);

% Открытие двоичного файла для чтения
fid = fopen(filepath, 'rb');
if fid == -1
    errordlg('Невозможно открыть указанный файл.', 'Ошибка');
    return;
end

% Чтение данных как int16
data = fread(fid, 'int16');

% Закрытие файла
fclose(fid);

% Проверка, что длина данных кратна 6
if mod(length(data), 6) ~= 0
    errordlg('Размер данных не кратен 6. Проверьте корректность файла данных.', 'Ошибка');
    return;
end

% Вычисление количества записей
num_records = length(data) / 6;

% Преобразование данных в матрицу N x 6
data = reshape(data, 6, num_records)';
% Столбцы соответствуют: [gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z]

% Извлечение данных гироскопа и акселерометра
gyro_data = data(:, 1:3);
acc_data = data(:, 4:6);

% Создание временного вектора с учетом частоты дискретизации
t = (0:num_records-1)' / fs; % Время в секундах

% Игнорирование первой секунды данных
ignore_samples = fs; % Количество записей, соответствующее первой секунде
if num_records <= ignore_samples
    errordlg('Длина записи меньше или равна одной секунде. Недостаточно данных для обработки.', 'Ошибка');
    return;
end
gyro_data = gyro_data(ignore_samples+1:end, :);
acc_data = acc_data(ignore_samples+1:end, :);
t = t(ignore_samples+1:end);

% Конвертация данных в физические величины
acc_data = acc_data * acc_g; % м/с²
gyro_data = gyro_data * gyro_g; % рад/с

% Расчет среднего значения для каждого сигнала
mean_acc = mean(acc_data);
mean_gyro = mean(gyro_data);

% Вычитание среднего значения из каждого сигнала
acc_data_centered = acc_data - mean_acc;
gyro_data_centered = gyro_data - mean_gyro;

% Расчет RMS для каждого сигнала относительно среднего уровня
rms_acc = rms(acc_data_centered);
rms_gyro = rms(gyro_data_centered);

% Создание фигуры на весь экран с панелью инструментов
figure('units','normalized','outerposition',[0 0 1 1], 'Toolbar', 'figure');

% Получение дескриптора панели инструментов
htoolbar = findall(gcf,'Type','uitoolbar');

% Отключение кнопки Pan и оставление только Zoom
% Поиск кнопки Pan
hpan = findall(htoolbar,'Tag','Exploration.Pan');
if ~isempty(hpan)
    delete(hpan); % Удаляем кнопку Pan
end

% Поиск кнопок Zoom
hzoom_in = findall(htoolbar,'Tag','Exploration.ZoomIn');
hzoom_out = findall(htoolbar,'Tag','Exploration.ZoomOut');

% Убедимся, что кнопки Zoom видимы
if ~isempty(hzoom_in)
    set(hzoom_in, 'Visible', 'on');
end
if ~isempty(hzoom_out)
    set(hzoom_out, 'Visible', 'on');
end

% Построение данных акселерометра и гироскопа в одном окне с отдельными графиками для каждого сигнала
ax1 = subplot(3,2,1);
plot(t, acc_data(:, 1), 'r');
xlabel('Время, сек');
ylabel('acc_x (м/с²)');
title(['Акселерометр - acc_x (Mean = ' num2str(mean_acc(1)) ', RMS = ' num2str(rms_acc(1)) ')']);
grid on;

ax2 = subplot(3,2,3);
plot(t, acc_data(:, 2), 'b');
xlabel('Время, сек');
ylabel('acc_y (м/с²)');
title(['Акселерометр - acc_y (Mean = ' num2str(mean_acc(2)) ', RMS = ' num2str(rms_acc(2)) ')']);
grid on;

ax3 = subplot(3,2,5);
plot(t, acc_data(:, 3), 'g');
xlabel('Время, сек');
ylabel('acc_z (м/с²)');
title(['Акселерометр - acc_z (Mean = ' num2str(mean_acc(3)) ', RMS = ' num2str(rms_acc(3)) ')']);
grid on;

ax4 = subplot(3,2,2);
plot(t, gyro_data(:, 1), 'r');
xlabel('Время, сек');
ylabel('gyro_x (град/с)');
title(['Гироскоп - gyro_x (Mean = ' num2str(mean_gyro(1)) ', RMS = ' num2str(rms_gyro(1)) ')']);
grid on;

ax5 = subplot(3,2,4);
plot(t, gyro_data(:, 2), 'b');
xlabel('Время, сек');
ylabel('gyro_y (град/с)');
title(['Гироскоп - gyro_y (Mean = ' num2str(mean_gyro(2)) ', RMS = ' num2str(rms_gyro(2)) ')']);
grid on;

ax6 = subplot(3,2,6);
plot(t, gyro_data(:, 3), 'g');
xlabel('Время, сек');
ylabel('gyro_z (град/с)');
title(['Гироскоп - gyro_z (Mean = ' num2str(mean_gyro(3)) ', RMS = ' num2str(rms_gyro(3)) ')']);
grid on;

% Синхронизация оси X для всех графиков
linkaxes([ax1, ax2, ax3, ax4, ax5, ax6], 'x');

% Фильтрация данных с помощью скользящего среднего с окном 30 секунд
window_size = 30 * fs; % количество отсчетов в окне 30 секунд
acc_data_filtered = movmean(acc_data, window_size);
gyro_data_filtered = movmean(gyro_data, window_size);

% Создание новой фигуры для отображения отфильтрованных данных акселерометра и гироскопа
figure('units','normalized','outerposition',[0 0 1 1], 'Toolbar', 'figure');

ax7 = subplot(3,2,1);
plot(t, acc_data_filtered(:, 1), 'r');
xlabel('Время, сек');
ylabel('acc_x (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc_x');
grid on;

ax8 = subplot(3,2,3);
plot(t, acc_data_filtered(:, 2), 'b');
xlabel('Время, сек');
ylabel('acc_y (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc_y');
grid on;

ax9 = subplot(3,2,5);
plot(t, acc_data_filtered(:, 3), 'g');
xlabel('Время, сек');
ylabel('acc_z (м/с²) - Фильтрованные');
title('Акселерометр (фильтрованные) - acc_z');
grid on;

ax10 = subplot(3,2,2);
plot(t, gyro_data_filtered(:, 1), 'r');
xlabel('Время, сек');
ylabel('gyro_x (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro_x');
grid on;

ax11 = subplot(3,2,4);
plot(t, gyro_data_filtered(:, 2), 'b');
xlabel('Время, сек');
ylabel('gyro_y (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro_y');
grid on;

ax12 = subplot(3,2,6);
plot(t, gyro_data_filtered(:, 3), 'g');
xlabel('Время, сек');
ylabel('gyro_z (град/с) - Фильтрованные');
title('Гироскоп (фильтрованные) - gyro_z');
grid on;

% Синхронизация оси X для всех графиков фильтрованных данных
linkaxes([ax7, ax8, ax9, ax10, ax11, ax12], 'x');

Очевидна идея вычитать из данных гироскопа постоянное смещение рассчитанное на основе данных в статичном состоянии. Хотя уже через 5 мин показания начнут вращаться со скростью 0.02 град в сек. Это уже больше градуса в минуту или полный кажущийся оборот за 5 мин. Тут неплохо бы иметь еще какой-либо датчик предотвращающий ложное вращение, например магнитометр. Но в этой статье не ставится задача обсуждения ориентирования в пространстве.

Можем ли мы этим IMU чипом определить поворот на доли градуса

Проведем эксперимет по повороту модуля на доли градуса по разным осям. Просто ложим модуль на жесткую рейку длиной 1.9 м с приподнятым одним концом на 25 мм. Через 2 мин опускаем приподнятый конец в горизонтальное положение. Это будет поворот на 0.75 градуса. Частота сэмплирования при этом 1666 Гц. Данные фильтруются бугущим средним с окном в 1 сек.

Вот график реакции на поворот 0.75 градуса в течении 1 сек по оси X

Опыты с акселерометром и гироскопом на платформе S7V30 - 16

Как видно, смена ориентации на 0.75 градуса достаточно хорошо фиксируется. По оси X аксклкрометра виден отчетливый скачок. значительно превышающий шумовой дрейф. Т.е. модуль вполне пригоден для косвенного измерения нагрузок по степени гибкой деформации или для достаточно точного позиционирования недлинных поворотных балок в промышленности.

Визуализация ориентации в пространстве в реальном времени

Теперь попытаемся реализовать на основе акселерометра и гироскопа визуализацию ориентации в пространстве нашего модуля. Используем для этого компонент Simulation 3D Actor среды Simulink и прямое соединение с модулем по протоколу TCP/IP. Как было показано в моих предыдущих статьях среда matlab вполне справляется с управлением внешними устройствами в реальном времени с тактом в 10 мс. В данном случае также выбираем такт 10 миллисекунд.

Сначала строим подсистему приёма данных

Опыты с акселерометром и гироскопом на платформе S7V30 - 17

Это подсистема подключается по протоколу tcp/ip на порт 3333 к модулю и получает от него сырые данные в виде 6 двухбайтных слов. Далее идёт синхронизация с тактом системы и демультиплексирование данных на канала акселерометра и гироскопа. К каналам акселерометра и гироскопа применяются множители для приведения величин к физическим значениям.

Построив приемник в подсистеме Receiver, достариваем остальную систему

Опыты с акселерометром и гироскопом на платформе S7V30 - 18

Подсистема Data Accumulation and Averaging собирает и усредняет данные от гироскопа за первую секунду записи. Три полученный константы будут применены для корректировки смещения в данных от гироскопа. Спустя секунду от начала записи включается подсистема 3D Orientation Estimation.

Опыты с акселерометром и гироскопом на платформе S7V30 - 19

Основную работу в этой подсистеме выполняет блок Complementary Filter. Комплементарный фильтр — это простой, линейный фильтр, подходящий для задач, где требуется только базовая стабилизация и ориентация. В Simulink есть и более сложные алгоритмы оценки ориентации, но пока их трогать не будем. Даже с комплементарным фильтром результат получился довольно интересный.

Опыты с акселерометром и гироскопом на платформе S7V30 - 20

На этом заканчиваю мой короткий обзор и представляю перечень использованных для этой статьи инструментов:

  • Altium Designer 24.10.1 - схема и плата

  • SolidWorks - 3D модель в формате STL для Simulation 3D Actor

  • Matlab 2024A - расчеты, графики и 3D визуализация

  • ChatGPT 4o - все скрипты для MATLAB

Прошивка для модуля S7V30 находится здесь.
Модель MATLAB Simulink здесь

Автор: Indemsys

Источник

* - обязательные к заполнению поля


https://ajax.googleapis.com/ajax/libs/jquery/3.4.1/jquery.min.js