ВВЕДЕНИЕ

Микроэлектромеханические MEMS датчики широко применяются в системах управления наземными, подводными и воздушными объектами, в навигации, смартфонах, игровых консолях. MEMS датчики используются для измерения угловых скоростей (гироскопы), ускорений (акселерометры), давления, влажности, температуры, концентрации химических веществ, газов.

Показания MEMS датчиков содержат ошибки, генерируемые шумами, нелинейностями, нестабильностью, взаимовлиянием координат, и др. источниками.

Существует и разрабатывается целый ряд методов минимизации ошибок. Оценки эффективности методов проводятся на показаниях MEMS датчиков, полученных при разных условиях эксплуатации.

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

В этой работе рассматриваются средства MATLAB для генерации показаний виртуальных инерциальных MEMS датчиков.

Этапы моделирования MEMS датчика

Схема моделирования показаний MEMS датчика на примере показаний accelReadings виртуального акселерометра, перемещающегося с ускорением acceleration [1] представлена на Рисунок 1. Показания датчика содержат ошибки, вызванные шумами, дрейфом нуля, взаимовлиянием координат, изменением температуры, квантованием и др. факторами.

Рисунок 1.Схема моделирования показаний accelReadings виртуального акселерометра, перемещающегося с ускорением acceleration [1].
Рисунок 1.Схема моделирования показаний accelReadings виртуального акселерометра, перемещающегося с ускорением acceleration [1].

Моделирование показаний MEMS датчика включает два основных этапа:

1.      Построение модели датчика (акселерометр – гироскоп, акселерометр – магнитометр, или акселерометр – гироскоп – магнитометр), которая содержит основные источники ошибок (Рисунок 2).

2.      Формирование и подача  воздействий на модель в виде линейных ускорений, угловых скоростей и ориентации датчика (Рисунок 3).

Рисунок 2. Построение модели MEMS датчика. Классы accelparams, gyroparams и magparams задают объекты параметров датчиков. Системный объект imusensor создает IMU модель MEMS датчика.
Рисунок 2. Построение модели MEMS датчика. Классы accelparams, gyroparams и magparams задают объекты параметров датчиков. Системный объект imusensor создает IMU модель MEMS датчика.
Рисунок 3. Моделирование показаний MEMS датчика. IMU модель датчика работает как функция, которая принимает входы (воздействия) и возвращает выходы (показания датчика).
Рисунок 3. Моделирование показаний MEMS датчика. IMU модель датчика работает как функция, которая принимает входы (воздействия) и возвращает выходы (показания датчика).

Создание объектов параметров датчиков модуля MPU-9250

3D акселерометр

Данные спецификации

3D акселерометр с 16 битным АЦП показывает линейные ускорения. Параметры акселерометра:

  • Диапазон: ± 2 ± 4 ± 8 ± 16g

  • Чувствительность в диапазонах ± 2, ± 4, ± 8, ± 16g: 16384, 8192, 4096, 2048 бит/g

  • Точность начальной калибровки: ± 3%

  • Чувствительность к температуре: ± 0.02%/°С (-40°C .. +85°C)

  • Нелинейность: 0.5%

  • Чувствительность к показаниям по другим осям: ± 2%

  • Показание нуля по X и Y:              ± 50 мg

  • Показание нуля по Z:                      ± 80 мg

  • Изменение X и Y нуля от температуры:   ± 35 мg (0°C .. +70°C)

  • Изменение Z нуля от температуры:          ± 60 мg (0°C .. +70°C)

  • Спектральная плотность шума:                 14 ug/sqrt(Гц)  (10 Гц, ± 2g, 1кГц АЦП)

  • Частота программируемого семплирования АЦП: от 4 до 1000 преобразований в секунду.

  • Показания акселерометра микросхемы MPU-6050 установленной в горизонтальной плоскости: 0g по X и Y осям и +1g по оси Z

Объект параметров accelparams

Класс accelparams задает параметры акселерометра для моделирования инерциального акселерометра с помощью объектной модели imuSensor.

Следующие параметры 3D акселерометра объединяются в объект.

accel_params = accelparams (...

'MeasurementRange',2*9.81,...           % m/s2 (default: Inf)

'Resolution', 2*9.81/2^16,...           % (m/s2)/LSB (default: 0)

'ConstantBias', [0 0 0],...             % m/s2 (default: [0 0 0])

'AxesMisalignment', [0 0 0],...         % % (default: [0 0 0])

'NoiseDensity', Na,...                  % (m/s2)/√Hz (default: [0 0 0])

'BiasInstability', Ba,...               % m/s2 (default: [0 0 0])

'RandomWalk', Ka,...                    % (m/s2)/√Hz (default: [0 0 0])

'TemperatureBias', [0 0 0],...          % (m/s2)/°C (default: [0 0 0])

'TemperatureScaleFactor', [0 0 0]);     % %/°C (default: [0 0 0])

 

Например,

Na = [1.4 1.4 2.5]*1e-3

Ka = [1.2318 1.517 2.3184]*1e-06

Ba = [1.4487 1.022 1.8373]*1e-04

>> accel_params = accelparams('MeasurementRange', 2*9.81,'Resolution', 2*9.81/2^16,'NoiseDensity', N, 'RandomWalk', K, 'BiasInstability', B);

 

3D гироскоп

Данные спецификации

3D гироскоп с 16 битным АЦП показывает угловую скорость. Параметры гироскопа:

  • Диапазон: ± 250, ± 500, ± 1000, ± 2000 °/с

  • Чувствительность в диапазонах ± 250, ± 500, ± 1000, ± 2000 °/с: 131, 65.5, 32.8, 16.4 бит/(°/с)

  • Изменение коэффициента пропорциональности:  < ± 3% (25 °C)

  • Чувствительность коэффициента пропорциональности к изменению температуры: ± 2%

  • Нелинейность: ± 0.2%

  • Чувствительность к показаниям по другим осям: ± 2%

  • Показания при нулевой скорости:                         ± 20 °/с (25 °С)

  • Зависимость показаний при нулевой скорости от температуры: ± 20 °/с (-40°C .. +85°C)

  • Чувствительность к линейным ускорениям: ± 0,1 (°/с)/g

  • Среднеквадратичный уровень шума (RMS):  0.05 °/с (± 250 °/с; 100 Гц)

  • Среднеквадратичный низкочастотный уровень шума (RMS):  0.033 °/с (± 250 °/с; 1-10 Гц)

  • Спектральная плотность шума (RMS):  0.005 °/с/sqrt(Гц) (± 250 °/с; 10 Гц)

  • Механические частоты

    • X                                30 (33) 36 кГц

    • Y                                27 (30) 33 кГц

    • Z                                 24 (27) 30 кГц

  • Частота программируемого семплирования АЦП: от 4 до 8000 преобразований в секунду.

  • Время установки показаний гироскопа после его включения: 30 мс (от подачи питания до ± 1 °/с от конечного значения)

 

Объект параметров

Класс gyroparams создает объект параметров гироскопа для моделирования инерциального гироскопа с помощью объектной модели imuSensor.

Объект включает следующие параметры модели гироскопа.

gyro_params = gyroparams( ...

'MeasurementRange',deg2rad(250), ...    % rad/s (default: Inf)

'Resolution', deg2rad(250/2^16), ...    % (rad/s)/LSB (default: 0)

'ConstantBias', [0 0 0], ...            % rad/s (default: [0 0 0])

'AxesMisalignment', [0 0 0], ...        % % (default: [0 0 0])

'NoiseDensity', Ng, ...                 % (rad/s)/√Hz (default: [0 0 0])

'BiasInstability', Bg, ...              % rad/s (default: [0 0 0])

'RandomWalk', Kg, ...                   % (rad/s)/√Hz (default: [0 0 0])

'TemperatureBias', [0 0 0],     ...     % (rad/s)/°C (default: [0 0 0])

'TemperatureScaleFactor', [0 0 0], ...  % %/°C (default: [0 0 0])

'AccelerationBias', [0 0 0]);           % (rad/s)/(m/s2)(default:[0 0 0])

 

Например,

Ng = deg2rad([86.5  91.6  92.9]*1e-3);

Kg = deg2rad([3.076  8.289  3.6737]*1e-05);

Bg = deg2rad([3.09  5.8  3]*1e-03);

>> gyro_params = gyroparams('MeasurementRange', deg2rad(250*2),'Resolution', deg2rad(250*2/2^16),'NoiseDensity', Ng, 'RandomWalk', Kg, 'BiasInstability', Bg);

 

 

3D магнитометр

Данные спецификации

3D магнитометр с 16 битным АЦП показывает индукцию магнитного поля вокруг датчика. Параметры магнитометра:

o   Диапазон измерения:                                                                >= +/- 4912 uT (25oC)

o   Встроенный АЦП:

  • Разрешение:                                                             14-/16-бит

  • Чувствительность:                                       0.57 (0.6) 0.63 µT/LSB typ. (14-бит)

0.1425 (0.15) 0.1575 µT/LSB typ. (16-бит)

  • Начальное смещение:                                                                     -500 .. +500 бит (25oC)

  • Время измерения:                                                                           (7.2) 9 мс

o   Имеет встроенный источник магнитного поля для калибровки датчика

 

Объект параметров

Класс (объект) magparams задает параметры магнитометра. Этот объект используется для моделирования магнитометра.

mag_params = magparams (...

'MeasurementRange', 4800,...            % uT (default: Inf)

'Resolution', 4800*2/2^16,...           % uT/LSB (default: 0)

'ConstantBias', [0 0 0],...             % uT (default: [0 0 0])

'AxesMisalignment', [0 0 0],...         % % (default:  [0 0 0])

'NoiseDensity', Nm,...                  % uT/√Hz (default: [0 0 0])

'BiasInstability', Bm,...               % uT (default: [0 0 0])

'RandomWalk', Km,...                    % uT/√Hz (default: [0 0 0])

'TemperatureBias', [0 0 0],...          % uT/°C (default: [0 0 0])

'TemperatureScaleFactor', [0 0 0]);     % %/°C (default: [0 0 0])

 

Например,

Nm = [0.6 0.6 0.9]/sqrt(100)

mag_params = magparams('MeasurementRange',4800,'Resolution', 4800*2/2^16, 'ConstantBias',1, 'NoiseDensity',Nm,'TemperatureBias',     [0.8 0.8 2.4], 'TemperatureScaleFactor',0.1);

 

Коэффициенты отклонения Аллана

Коэффициенты N, B, K отклонения Аллана пропорциональны следующим ошибкам MEMS датчиков:

·         N - случайное блуждание по углу (ARW - Angle Random Walk),

·         B - нестабильность смещения (BI - Bias Instability),

·         K - случайное блуждание по скорости (RRW - Rate Random Walk),

Коэффициенты контретного датчика можно вычислить по наклонам графика adev отклонения Аллана. Отклонения вычисляются для заданного количества точек m по долговременным показаниям датчика, полученными с частотой опроса Fs.

Отклонения Аллана равны квадратному корню от дисперсии Аллана:

[avar,tau] = allanvar(omega,'octave',Fs); % avar(tau) дисперсия Аллана

или

[avar, tau] = allanvar(omega, m, Fs);

Рисунок 4. Пример отклонения Аллана с наклонами N, B, K (слева) и сравнение отклонений Аллана (справа), вычисленных последовательно (синяя линия) и c применением функции MATLAB allanvar (красная штрихпунктирная линия SIM).
Рисунок 4. Пример отклонения Аллана с наклонами N, B, K (слева) и сравнение отклонений Аллана (справа), вычисленных последовательно (синяя линия) и c применением функции MATLAB allanvar (красная штрихпунктирная линия SIM).

Зависимость коэффициентов Аллана от частоты семплирования Fs

 Таблица 1. Изменение Fs прореживанием массива долговременных показаний

 

Fs = 2

Fs = 0.2

Fs = 0.02

Fs = 0.002

Fs = 0.0002

N

K

B

 0.0865

3.0761e-05

 0.0031

0.2741

3.3292e-05

0.0042

0.8717

3.9661e-05

0.0080

2.8123

1.0783e-04

0.0265

15.4488

4.2866e-04

0.0737

При прореживании показаний в 100 раз, приводящему к такому же уменьшению частоты семплирования, на участке изменения Fs от 2 до 0.02 Гц

N увеличивается в 10 раз

K увеличивается на 25%

B увеличивается в 2.7 разa

 

Таблица 2. Изменение Fs при сохранении массива долговременных показаний  

 

Fs = 2

Fs = 0.2

Fs = 20

Fs = 200

Fs = 2000

N

K

B

 0.0865

3.0761e-05

 0.0031

0.2737

9.7273e-06

0.0031

0.0274

9.7273e-05

0.0031

 0.0087

3.0761e-04

0.0031

0.0027

9.7273e-04

0.0031

При увеличении частоты Fs в 100 раз от 2 до 200 Гц

N уменьшается в 10 раз

K увеличивается в 10 раз

B остается без измерения

 

Создание модели MEMS датчика

Для моделирования показаний датчика необходимо создать системный объект модели imuSensor c заданными свойствами, а затем работать с объектом, как если бы это была функция.

Виртуальный модуль MEMS модуля может включать следующие типы датчиков.

'accel-gyro'

'accel-mag'

'accel-gyro-mag'

Варианты создания IMU объектов MEMS датчика c перечисленными выше структурами:

% Создание модели 'accel-gyro' MEMS датчика

IMU_accel_gyro = imuSensor( ...

'IMUType', 'accel-gyro', ...                    % 'accel-gyro' (default)'

'SampleRate', Fs, ...                           % 100 (default)

'Temperature', 25, ...                          % 25 (default)

'Accelerometer', accel_params, ...              % accel_params object (default)

'Gyroscope', gyro_params, ...                   % gyro_params object (default)

'RandomStream', 'Global stream');                % 'Global stream' (default) | 'mt19937ar with seed'

 

% Создание модели 'accel-mag' MEMS датчика

IMU_accel_mag = imuSensor( ...

'IMUType', 'accel-mag', ...                     % 'accel-gyro' (default)'

'SampleRate', Fs, ...                           % 100 (default)

'Temperature', 25, ...                          % 25 (default)

'MagneticField', [27.5550 -2.4169 -16.0849],... % [27.5550 -2.4169 -16.0849] (default)

'Accelerometer', accel_params, ...              % accel_params object (default)

'Magnetometer', mag_params, ...                 % mag_params object (default)

'RandomStream', 'Global stream');                % 'Global stream' (default) | 'mt19937ar with seed'

 

% Создание модели 'accel-gyro_mag' MEMS датчика

IMU_accel_gyro_mag = imuSensor( ...

'IMUType', 'accel-gyro-mag', ...                % 'accel-gyro' (default)'

'SampleRate', Fs, ...                           % 100 (default)

'Temperature', 25, ...                          % 25 (default)

'MagneticField', [27.5550 -2.4169 -16.0849],... % [27.5550 -2.4169 -16.0849] (default)

'Accelerometer', accel_params, ...              % accel_params object (default)

'Gyroscope', gyro_params, ...                   % gyro_params object (default)

'Magnetometer', mag_params, ...                 % mag_params object (default)

'RandomStream', 'Global stream');               % 'Global stream' (default) | 'mt19937ar with seed'

 Например:

Fs= 1000;

IMU_accel_gyro = imuSensor('SampleRate', Fs, 'Accelerometer', accel_params, 'Gyroscope', gyro_params); 

IMU_accel_gyro_mag = imuSensor('accel-gyro-mag','SampleRate', Fs, 'Accelerometer', accel_params, 'Gyroscope', gyro_params, 'Magnetometer', mag_params);

 

Моделирование показаний MEMS датчика

Варианты моделей IMU:

[accelReadings,gyroReadings] = IMU(acc_body,angVel_body)

[accelReadings,gyroReadings] = IMU(acc_body,angVel_body,orientation_body)

[accelReadings,magReadings] = IMU(acc_body,angVel_body)

[accelReadings,magReadings] = IMU(acc_body,angVel_body,orientation_body)

[accelReadings,gyroReadings,magReadings] = IMU(acc_body,angVel_body)

[accelReadings,gyroReadings,magReadings] = IMU(acc_body,angVel_body,orientation_body)

% accelReadings [m/s2], gyroReadings [rad/s], magReadings [uT]

Модели позволяют моделировать показания виртуальных датчиков: акселерометров accelReadings, гироскопов gyroReadings, и магнитометров magReadings, соответствующие воздействиям и характеристикам (параметрам) датчиков.

На вход моделей подаются ускорения датчика acc по трем координатам, угловые скорости  angVel по трем координатам и, опционно, положение в пространстве orientation в кватернионах, например, 0.99953 +  0.010297i - 0.0093976j +  0.027471k или матрицах вращения.

 

Показания 3D акселерометра

Задание траекторий ускорения датчика 

Fs = 1000;                               % частота семплирования

numSamples = 10000;                      % количество показаний

t = 0:1/Fs:(numSamples-1)/Fs;            % время считывания показаний

ang_vel_body = zeros(numSamples, 3);     % массив 3D угловых скоростей

acc_body = zeros(numSamples, 3);         % массив 3D линейных ускорений

acc_x = 0.02*(2*pi/3.3)^2*sin(6*pi*t/10); % ускорение вдоль X, период 3.3с

acc_body(:,1) = acc_x;

 

Сравнение показаний модели и реального акселерометра

% создание объекта IMU модели

IMU = imuSensor('accel-gyro','SampleRate', Fs, 'Accelerometer', accel_params, 'Gyroscope', gyro_params);

% моделирование показаний accelReadings акселерометра и gyroReadings гироскопа

[accelReadings,gyroReadings] = IMU(acc_body,ang_vel_body);

% accelReadings [m/s2], gyroReadings [rad/s]

 На Рисунок 5 показаны смоделированные и реальные показания акселерометра X и их стандартные отклонения.

Рисунок 5. Смоделированные (первые два ряда) и реальные (два нижних рисунка) показания акселерометра X и их стандартные отклонения.
Рисунок 5. Смоделированные (первые два ряда) и реальные (два нижних рисунка) показания акселерометра X и их стандартные отклонения.

 ВЫВОДЫ

1.      Стандартное отклонение шумов (Рисунок 5) смоделированных показаний акселерометра  = [0.002 0.005 0.014 0.044] увеличивается с ростом частоты семплирования Fs  = [2 10 100 1000] в √(Fs/2)  раз.

2.      Стандартное отклонение шумов реального датчика не изменяется c увеличением частоты семплирования от 2 Гц до 1000 Гц.

3.      Стандартное отклонение stdm  смоделированных показаний акселерометра совпадает со стандартным отклонением stdsens  показаний датчика когда оба варианта получены при одинаковой частоте семплирования Fsens  и коэффициенты Аллана модели Nmb и Kmb больше соответствующих коэффициентов реального датчика Nsens и Ksens в √(Fsens)  раз.

Например, если по долговременным показаниям реального датчика на частоте семплирования Fsens = 2 Hz вычислены коэффициенты Аллана Nsens = 0.0014  и Ksens = 1.2318e-06, то для моделирования показаний с тем же стандартным отклонением необходимо установить коэффициенты Аллана

N_{mb} = N_{sens} √F_{sens} = 0.0014√2 = 0.002K_{mb} = K_{sens} √F_{sens} = 1.2318e-06√2 = 1.724e-06

4.      При постоянных N, K коэффициентах Аллана, увеличение частоты Fsm  семлирования модели относительно частоты Fsens  семлирования датчика приводит к росту стандартного отклонения stdm модели относительно стандартного отклонения stdsens:

std_m= std_{sens} √(F_{sm}/F_{sens} )

как показано на Рисунок 5.

5.      Для обеспечения равенства между стандартным отклонением stdm модели и stdsens  датчика при частоте семплирования модели Fsm отличающейся от частоты семплирования датчика Fsens необходимо задать коэффициенты Аллана модели Nm и Km  с учетом N, K коэффициентов датчика:

N_m=N F_{sens}/√F_{sm} ; K_m=K F_{sens}/√F_{sm} ;

Рисунок 6. Смоделированные показания акселерометра с частотой семплирования 2 Гц, N = 1.4e-3, K = 1.2318e-6 (слева) и 200 Гц (100*2Гц ) , N = 0.198e-3 , K = 0.174e-6 (справа).
Рисунок 6. Смоделированные показания акселерометра с частотой семплирования 2 Гц, N = 1.4e-3, K = 1.2318e-6 (слева) и 200 Гц (100*2Гц ) , N = 0.198e-3 , K = 0.174e-6 (справа).

Показания 3D гироскопа

Задание траекторий угловой скорости датчика

 

Fs = 1000;                               % частота семплирования

numSamples = 10000;                      % количество показаний

t = 0:1/Fs:(numSamples-1)/Fs;            % время считывания показаний

acc_body = zeros(numSamples, 3);          % массив 3D линейных ускорений

ang_vel_body = zeros(numSamples, 3);     % массив 3D угловых скоростей

ang_vel_x = 20*(2*pi/3.3)*sin(6*pi*t/10); % скорость поворота вокруг X

ang_vel_body(:,1) = ang_vel_x;

 

Сравнение показаний модели и реального гироскопа

% создание объекта IMU модели

IMU = imuSensor('accel-gyro','SampleRate', Fs, 'Accelerometer', accel_params, 'Gyroscope', gyro_params);

% моделирование показаний accelReadings акселерометра и gyroReadings гироскопа

[accelReadings,gyroReadings] = IMU(acc_body,ang_vel_body);

% accelReadings [m/s2], gyroReadings [rad/s]

 

[accelReadings,gyroReadings] = IMU(acc_body,ang_vel_body);

% accelReadings [m/s2], gyroReadings [rad/s

ВНИМАНИЕ! При моделировании показаний, коэффициенты Аллана Т и К, а также угловая скорость модели датчика задаются в рад/с, а не в град/с.

Рисунок 7. Смоделированные и реальные (нижний ряд) показания гироскопов. Амплитуда шумов смоделированных показаний датчика пропорциональна частоте семплирования с коэффициентом k*√Fs
Рисунок 7. Смоделированные и реальные (нижний ряд) показания гироскопов. Амплитуда шумов смоделированных показаний датчика пропорциональна частоте семплирования с коэффициентом k*√Fs

ВЫВОДЫ

1.    Стандартное отклонение шумов (Рисунок 7) смоделированных показаний гироскопа stdm  = [0.124 0.298 0.889 2.725] увеличивается с ростом частоты семплирования Fs = [2 10 100 1000] в √(Fs/2)  раз.

2.    Стандартное отклонение шумов реального датчика не изменяется c увеличением частоты семплирования от 2 Гц до 1000 Гц.

3.    Стандартное отклонение stdm  смоделированных показаний гироскопа совпадает со стандартным отклонением stdsens  показаний датчика когда оба варианта получены при одинаковой частоте семплирования Fsens  и коэффициенты Аллана модели Nmb  и Kmb  больше соответствующих коэффициентов реального датчика  Nsens и Ksens  в √Fsens  раз.

Например, если по долговременным показаниям реального датчика на частоте семплирования Fsens = 2 Hz вычислены коэффициенты Аллана Nsens = 0.0865 и Ksens = 3.076e-05, то для моделирования показаний с тем же стандартным отклонением необходимо установить коэффициенты Аллана

N_{mb} = N_{sens} √F_{sens} = 0.0865√2 = 0.1223K_{mb} = K_{sens} √F_{sens} = 3.076e-05√2 = 4.35e-05

4.    При постоянных N, K коэффициентах Аллана, увеличение частоты Fsm  семлирования модели относительно частоты Fsens  семлирования датчика приводит к росту стандартного отклонения stdm  модели относительно стандартного отклонения stdsens :

std_m= std_{sens} √(F_{sm}/F_{sens})

как показано на Рисунок 7.

5.    Для обеспечения равенства между стандартным отклонением stdm  модели и stdsens  датчика при частоте семплирования модели Fsm  отличающейся от частоты семплирования датчика Fsens  необходимо задать коэффициенты Аллана модели Nm, Km  с учетом N, K коэффициентов датчика:

N_m=N F_{sens}/√F_{sm}; K_m=K F_{sens}/√F_{sm};
Рисунок 8. Смоделированные показания гироскопа с частотой семплирования 2 Гц, N = 0.0865, K = 3.076e-5 (слева) и 200 Гц (100*2Гц ) , N = 0.0122, K = 0.4356e-5 (справа).
Рисунок 8. Смоделированные показания гироскопа с частотой семплирования 2 Гц, N = 0.0865, K = 3.076e-5 (слева) и 200 Гц (100*2Гц ) , N = 0.0122, K = 0.4356e-5 (справа).

 

 

Показания 3D магнитометра

Моделирование показаний по угловой скорости датчика

Показания магнитометра зависят от поворотов датчика, которые моделируются угловыми скоростями датчика ang_vel_body.

acc_body = zeros(numSamples, 3);            % массив 3D линейных ускорений

ang_vel_body = zeros(numSamples, 3);        % массив 3D угловых скоростей

 

acc_x = 0.02*(2*pi/3.3)^2*sin(6*pi*t/10);   % ускорение вдоль X, период 3.3с

ang_vel_x = 20*(2*pi/3.3)*sin(6*pi*t/10);   % скорость поворота вокруг X

ang_vel_body(:,2) = ang_vel_x;

 

[accelReadings,magReadings] = IMU(acc_body,ang_vel_body);

% accelReadings [m/s2], magReadings [uT]

 

Пример показаний виртуального магнитометра показан на Рисунок 9. Показания магнитометра и амплитуда шумов в точности совпадают с задаваемой угловой скоростью датчика ang_vel_body и шумами модели гироскопа (Рисунок 8).

Рисунок 9. Показания магнитометра смоделированные по угловой скорости датчика.
Рисунок 9. Показания магнитометра смоделированные по угловой скорости датчика.

Моделирование показаний магнитометра через ориентацию датчика

Fcapt = 134;  % частота семплирования показаний магнетометра

Fs = 134;     % частота семплирования показаний модели

Nm = [1.9 1.9 1.9]*Fcapt/sqrt(Fs)

mag_params = magparams('MeasurementRange',4800,'Resolution', 4800*2/2^16, 'ConstantBias', [-3100 -1200 3010], 'NoiseDensity',Nm);

orientation_body = quaternion.ones(numSamples, 1); % val = 1 + 0i + 0j + 0k

acc_body = zeros(numSamples, 3);            % массив 3D линейных ускорений

ang_vel_body = zeros(numSamples, 3);        % массив 3D угловых скоростей

IMU = imuSensor('accel-mag','SampleRate',Fs); % объект модели

[accelReadings,magReadings] = IMU(acc_body,ang_vel_body,orientation_body); % magReadings in [мкТл]

Рисунок 10. Смоделированные показания магнитометра (слева) и показания реального магнитометра (справа).
Рисунок 10. Смоделированные показания магнитометра (слева) и показания реального магнитометра (справа).

В моделях 'accel-mag' и 'accel-gyro_mag' амплитуда магнитной индукции устанавливается через свойство модели 'MagneticField' , для показаний магнетометра (Рисунок 11) установлены значения амплитуды [-3000 -1100 3000], смещение показаний 'ConstantBias' в списке параметров объекта magparams равно [0 0 0].

Независимые (без модели магнитометра) вычисления магнитной индукции :

magReadings(t,Z)=√((MagneticField_X^2+MagneticField_Y^2)) ×sin∫angvelbody(:,3)dt

где ang_vel_body - скорость поворота датчика в [рад/с].

Для совпадения показаний модели Z магнитометра и реального магнитометра (Рисунок 10) вместо 'ConstantBias', [-3100 -1200 3010] в списке параметров объекта magparams заданы смещения модели

'ConstantBias',[0 0 0].

Для соответствия уровня шумов модели магнитометра шумам реального датчика (std = 170 [мкТл], Рисунок 10) коэффициенты дисперсии Аллана 'NoiseDensity' объекта параметров равны

Nm = [22, 22, 22]*√( F_{sens}/F_{s} ) [мкТл/√Гц],

где Fsens  = 134 Гц – частота опроса реального магнитометра; Fs – частота опроса модели магнитометра.

Показание модели (шумы и амплитуда сигнала) модели магнитометра (Рисунок 11) адекватны показанию реального магнитометра Рисунок 10. Стандартное отклонение показаний модели не зависит от частоты семплирования.

Рисунок 11. Показания модели магнитометра, при поворотах вокруг Z.
Рисунок 11. Показания модели магнитометра, при поворотах вокруг Z.

При поворотах (изменении ориентации) магнитометра вокруг оси Z изменяются показания магнитометров X и Y (Рисунок 11), показания линейных акселерометров не меняются. При поворотах магнитометра вокруг X и Z изменяются показания магнитометров и акселерометров (Рисунок 12).

Рисунок 12. Показания модели магнитометра, при поворотах (изменении ориентации) вокруг Y.
Рисунок 12. Показания модели магнитометра, при поворотах (изменении ориентации) вокруг Y.

Моделирование ориентации датчика

Ориентация изменяет чувствительность акселерометров. Ориентация модели датчика вычисляется в кватернионах или 3х3 матрицами поворота для каждого показания.

Преобразование кватерниона, вектора поворота и угла Эйлера

Преобразование кватерниона в вектор поворота и угол Эйлера и обратные преобразования выполняются следующими функциями MATLAB.

 

q = [0.7071 0.7071 0 0]

quat = quaternion(q)   % создание кватерниона

q = 0.7071  0.7071            0     0

norm(quat) = 1

 

rotationVector = rotvec(quat)  % преобразование кватерниона в вектор поворота (рад)

rotationVector = 1.5708      0     0

quat_back = quaternion(rotationVector,'rotvec') % обратное преобразование вектора поворота (рад) в кватернион

quat_back = 0.70711 + 0.70711i + 0j + 0k

 

rotationVectord = rotvecd(quat) % преобразование кватерниона в вектор поворота (град)

rotationVectord =  90     0     0

quat_back = quaternion(rotationVectord,'rotvecd') % обратное преобразование вектора поворота (град) в кватернион

quat_back = 0.70711 + 0.70711i + 0j + 0k

 

 

eulerAnglesRandians = euler(quat,'ZYX','frame') % Преобразование кватерниона в угол Эйлера (рад)

eulerAnglesRandians = 0      0     1.5708

quat_back = quaternion(eulerAnglesRandians,'euler','ZYX','frame') % обратное преобразование угол Эйлера (рад) в кватернион

quat_back = 0.70711 + 0.70711i + 0j + 0k

 

 

eulerAnglesDegrees = eulerd(quat,'XYZ','frame') % Преобразование кватерниона в угол Эйлера (град)

eulerAnglesDegrees = 90.0000       0     0

quat_back = quaternion(eulerAnglesDegrees,'eulerd','XYZ','frame') % обратное преобразование угол Эйлера (uрад) в кватернион

quat_back = 0.70711 + 0.70711i + 0j + 0k

 

 

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

Формирование ориентации моделью imufilter

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

Создание объекта FUSE модели imufilter :

FUSE = imufilter (...

'SampleRate', 100,...                   % 100 (default), (Hz)

'DecimationFactor', 1,...               % 1 (default)

'AccelerometerNoise', 0.00019247,...    % 0.00019247 (default), (m/s2)^2

'GyroscopeNoise', 9.1385e-05,...        % 9.1385e-05 (default) (rad/s)^2

'GyroscopeDriftNoise', 3.0462e-13,...   % 3.0462e-13 (default) (rad/s)^2

'LinearAccelerationNoise', 0.0096236,...% 0.0096236 (default) (m/s2)^2

'LinearAccelerationDecayFactor', 0.5,...% 0.5 (default)

'OrientationFormat', 'quaternion');     % 'quaternion' (default) | 'Rotation matrix'; 3x3xN  

Например

FUSE = imufilter('OrientationFormat', 'quaternion')

[orientation,angularVelocity] = FUSE(acc_body,ang_vel_body)

quaterionEulerAngles = eulerd(orientation_body,'XYZ','frame')

ВНИМАНИЕ.

1.      Угловая скорость ang_vel_body задается в рад/с

2.      Ориентация в углах Эйлера равна отношению угловой скорости к частоте семплирования:

quaterionEulerAngles [град]= (angvelbody [град])/'SampleRate'

3.      Ориентация вычисляется относительно координат акселерометра, например изменение вектора ускорений acc_body = [0 0 9.81] до [0 9.81 0] изменяет ориентацию вокруг X на 90 градусов.

Моделирование ориентации по показаниям акселерометров и магнитометров, ecompass()

 

accelerometerReading = [0 0 9.81];       % [м/с2]

magnetometerReading = [109 -23 9807];    % [нТл]

 

% ориентация в кватернионах

orientation_body = ecompass(accelerometerReading,magnetometerReading)

      % orientation_body =  0.9946 +     0i + 0j + 0.10379k

quaterionEulerAngles = eulerd(orientation_body,'ZYX','frame')

% quaterionEulerAngles = 11.9151   0     0

 

Формирование ориентации моделью kinematicTrajectory

Для моделирования ориентации orientationNED  по угловой скорости и линейному ускорению инерциального датчика необходимо создать объект kinematicTrajectory с заданными свойствами, а затем, использовать объект как функцию, которая вычисляет ориентацию кватернионами или матрицами поворота в соответствии с угловыми скоростями ang_vel_body и ускорениями acc_body.

 

traj = kinematicTrajectory(...

'SampleRate', 100, ...                  % 100 (default) (Hz)

'Position', [0 0 0], ...                % [0 0 0] (default) (m)

'Velocity', [0 0 0], ...                % [0 0 0] (default)  (m/s)

'Orientation', quaternion(1,0,0,0), ... % quaternion(1,0,0,0) (default) | scalar quaternion | 3-by-3 real matrix

'AccelerationSource', 'Input', ...      % 'Input' (default)

'AngularVelocitySource', 'Input');      % 'Input' (default)

 

[~,orientationNED,~,accNED,angVelNED] = traj(acc_body,ang_vel_body);

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

 

БИБЛИОГРАФИЧЕСКИЙ СПИСОК

 1.      Help MATLAB