Sensor Fusion and Tracking Toolbox™ позволяет вам сплавить данные, считанные с инерциального измерительного блока (IMU), чтобы оценить ориентацию и скорость вращения:
ecompass
-- Показания предохранителя и магнитометра
imufilter
-- Показания предохранителя и гироскопа
ahrsfilter
-- Показания предохранителя, гироскопа и магнитометра
Больше датчиков на БИНС приводят к более устойчивой оценке ориентации. Данные о датчике могут быть перекрестно проверены, и информация, которую передают датчики, ортогональна.
В этом руководстве представлен обзор слияния инерционных датчиков для БИНС в Sensor Fusion and Tracking Toolbox.
Чтобы узнать, как смоделировать инерционные датчики и GPS, смотрите Model IMU, GPS и INS/GPS. Чтобы узнать, как сгенерировать движение «земля-правда», которое управляет моделями датчика, смотрите waypointTrajectory
и kinematicTrajectory
.
Можно также сплавить показания БИНС с показаниями GPS для оценки положения. См. раздел «Определение положения при помощи инерционных датчиков и GPS» для получения обзора.
Этот пример показывает, как использовать 6-осевой и 9-осевой алгоритмы слияния для вычисления ориентации. Существует несколько алгоритмов для вычисления ориентации из инерционных модулей измерения (БИНС) и магнитно-угловых модулей силы тяжести (MARG). Этот пример охватывает основы ориентации и как использовать эти алгоритмы.
Ориентация
Ориентация объекта описывает его поворот относительно некоторой системы координат, иногда называемой родительской системой координат, в трёх размерностях.
Для следующих алгоритмов используется фиксированная, родительская система координат North-East-Down (NED). NED иногда упоминается как глобальная система координат или опорная рамка. В опорной системе координат NED ось X указывает на север, ось Y - на восток, а ось Z - на вниз. Плоскость X-Y NED рассматривается как локальная тангенциальная плоскость Земли. В зависимости от алгоритма север может быть либо магнитным севером, либо истинным севером. Алгоритмы в этом примере используют магнитным севером.
Если задано, следующие алгоритмы могут оценить ориентацию относительно родительской системы координат East-North-Up (ENU) вместо NED.
Объект может рассматриваться как имеющий свою собственную систему координат, часто называемую локальной или дочерней системой координат. Эта дочерняя система координат вращается вместе с объектом относительно родительской системы координат. Если перемещение отсутствует, источники обеих систем координат перекрываются.
Вычисленная величина ориентации является поворотом, который забирает величины из родительской опорной системы координат в дочернюю опорную систему координат. Вращение представлено кватернионом или матрицей вращения.
Типы датчиков
Для оценки ориентации обычно используются три типа датчиков: акселерометры, гироскопы и магнитометры. Акселерометры измеряют правильное ускорение. Гироскопы измеряют скорость вращения. Магнитометры измеряют локальное магнитное поле. Различные алгоритмы используются, чтобы сплавить различные комбинации датчиков для оценки ориентации.
Данные о датчике
В большинстве из этих примеров используется тот же набор данных о датчике. Данные о датчике акселерометра, гироскопа и магнитометра были записаны во время вращения устройства вокруг трех различных осей: сначала вокруг локальной оси Y, затем вокруг оси Z и, наконец, вокруг оси X. Ось X устройства в целом была направлена в южном направлении на время эксперимента.
ld = load('rpy_9axis.mat');
acc = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;
mag = ld.sensorData.MagneticField;
viewer = HelperOrientationViewer;
Синтаксис акселерометра-магнитометра
The ecompass
функция предохраняет данные акселерометра и магнитометра. Это алгоритм без памяти, который не требует настройки параметра, но алгоритм очень восприимчив к шуму датчика.
qe = ecompass(acc, mag); for ii=1:size(acc,1) viewer(qe(ii)); pause(0.01); end
Обратите внимание, что ecompass
алгоритм правильно находит местоположение севера. Однако, поскольку функция незабываема, предполагаемое движение не гладко. Алгоритм может использоваться как шаг инициализации в ориентационном фильтре или некоторые методы, представленные в Lowpass Filter Orientation Using Quaternion SLERP, могут использоваться, чтобы сглаживать движение.
Синтаксис акселерометра-гироскопа
Следующие объекты оценивают ориентацию, используя либо фильтр Калмана состояния ошибки, либо дополнительный фильтр. Фильтр Калмана состояния ошибки является стандартным фильтром оценки и позволяет настраивать многие различные аспекты системы с использованием соответствующих параметров шума. Дополнительный фильтр может использоваться в качестве замены для систем с ограничениями памяти и имеет минимальные настраиваемые параметры, что позволяет легче настроить за счет более тонкой настройки.
The imufilter
и complementaryFilter
Система objects™ данные акселерометра предохранителя и гироскопа. The imufilter
использует внутренний фильтр Калмана состояния ошибки и complementaryFilter
использует дополнительный фильтр. Фильтры способны устранить шум смещения гироскопа, который дрейфует со временем.
ifilt = imufilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qimu = ifilt(acc(ii,:), gyro(ii,:)); viewer(qimu); pause(0.01); end
% Disable magnetometer input. cfilt = complementaryFilter('SampleRate', ld.Fs, 'HasMagnetometer', false); for ii=1:size(acc,1) qimu = cfilt(acc(ii,:), gyro(ii,:)); viewer(qimu); pause(0.01); end
Хотя и imufilter
и complementaryFilter
алгоритмы дают значительно более плавные оценки движения, по сравнению с ecompass
, они не правильно оценивают направление севера imufilter
не обрабатывает данные магнитометра, поэтому просто принимает, что ось X устройства первоначально указывает на север. Оценка движения, заданная imufilter
- относительно начальной расчетной ориентации. The complementaryFilter
делает то же предположение, когда HasMagnetometer
для свойства задано значение false
.
Синтаксис акселерометра-гироскопа-магнитометра
Система ссылки ориентации и курса (AHRS) состоит из 9-осевой системы, которая использует акселерометр, гироскоп и магнитометр для вычисления ориентации. The ahrsfilter
и complementaryFilter
Система objects™ объединить лучшие из предыдущих алгоритмов, чтобы получить плавно изменяющуюся оценку ориентации устройства, при этом правильно оценив направление севера complementaryFilter
использует тот же алгоритм дополнительного фильтра, что и ранее, с дополнительным шагом, чтобы включить магнитометр и улучшить оценку ориентации. Как imufilter
, ahrsfilter
алгоритм также использует фильтр Калмана в состоянии ошибки. В дополнение к удалению смещения гироскопа, ahrsfilter
имеет некоторую способность обнаруживать и отвергать мягкие магнитные помехи.
ifilt = ahrsfilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qahrs = ifilt(acc(ii,:), gyro(ii,:), mag(ii,:)); viewer(qahrs); pause(0.01); end
cfilt = complementaryFilter('SampleRate', ld.Fs); for ii=1:size(acc,1) qahrs = cfilt(acc(ii,:), gyro(ii,:), mag(ii,:)); viewer(qahrs); pause(0.01); end
Параметры фильтра настройки
The complementaryFilter
, imufilter
, и ahrsfilter
Все системные objects™ имеют настраиваемые параметры. Настройка параметров на основе используемых заданных датчиков может улучшить эффективность.
The complementaryFilter
параметры AccelerometerGain
и MagnetometerGain
может быть настроена, чтобы изменить величину, которую каждые измерения датчика влияют на оценку ориентации. Когда AccelerometerGain
установлено в 0
для ориентации по осям X и Y используется только гироскоп. Когда AccelerometerGain
установлено в 1
для ориентации по осям X и Y используется только акселерометр. Когда MagnetometerGain
установлено в 0
для ориентации по оси Z используется только гироскоп. Когда MagnetometerGain
установлено в 1
для ориентации по оси Z используется только магнитометр.
The ahrsfilter
и imufilter
Системные objects™ имеют больше параметров, которые могут позволить фильтрам более точно совпадать с конкретными аппаратными датчиками. Окружение датчика также важно учитывать. The imufilter
параметры являются подмножеством ahrsfilter
параметры. The AccelerometerNoise
, GyroscopeNoise
, MagnetometerNoise
, и GyroscopeDriftNoise
являются шумами измерения. Таблицы данных датчиков помогают определить эти значения.
The LinearAccelerationNoise
и LinearAccelerationDecayFactor
управляйте реакцией фильтра на линейное (поступательное) ускорение. Встряхивание устройства является простым примером добавления линейного ускорения.
Рассмотрим, как imufilter
с LinearAccelerationNoise
9e-3 реагирует на трясущую траекторию, по сравнению с одной с LinearAccelerationNoise
9e-4.
ld = load('shakingDevice.mat'); accel = ld.sensorData.Acceleration; gyro = ld.sensorData.AngularVelocity; viewer = HelperOrientationViewer; highVarFilt = imufilter('SampleRate', ld.Fs, ... 'LinearAccelerationNoise', 0.009); qHighLANoise = highVarFilt(accel, gyro); lowVarFilt = imufilter('SampleRate', ld.Fs, ... 'LinearAccelerationNoise', 0.0009); qLowLANoise = lowVarFilt(accel, gyro);
Один из способов увидеть эффект LinearAccelerationNoise
- чтобы просмотреть выходной вектор силы тяжести. Вектор гравитации является просто третьим столбцом матрицы поворота ориентации.
rmatHigh = rotmat(qHighLANoise, 'frame'); rmatLow = rotmat(qLowLANoise, 'frame'); gravDistHigh = sqrt(sum( (rmatHigh(:,3,:) - [0;0;1]).^2, 1)); gravDistLow = sqrt(sum( (rmatLow(:,3,:) - [0;0;1]).^2, 1)); figure; plot([squeeze(gravDistHigh), squeeze(gravDistLow)]); title('Euclidean Distance to Gravity'); legend('LinearAccelerationNoise = 0.009', ... 'LinearAccelerationNoise = 0.0009');
The lowVarFilt
имеет низкую LinearAccelerationNoise
, поэтому он рассчитывает находиться в окружении с низким линейным ускорением. Поэтому он более восприимчив к линейному ускорению, как проиллюстрировано большими изменениями ранее на графике. Однако, поскольку он рассчитывает находиться в окружении с низким линейным ускорением, более высокое доверие помещается в сигнал акселерометра. Таким образом, оценка ориентации быстро сходится назад к вертикали, когда встряхивание закончится. Обращение верно для highVarFilt
. Фильтр меньше зависит от тряски, но оценка ориентации занимает больше времени, чтобы сходиться к вертикали, когда тряска остановилась.
The MagneticDisturbanceNoise
свойство позволяет моделировать магнитные нарушения порядка (негеомагнитные источники шума) почти таким же образом LinearAccelerationNoise
моделирует линейное ускорение.
Два свойства фактора распада (MagneticDisturbanceDecayFactor
и LinearAccelerationDecayFactor
) моделируют скорость изменения шумов. Для медленно изменяющихся источников шума установите эти параметры на значение ближе к 1. Для быстро изменяющихся, некоррелированных шумов установите эти параметры ближе к 0. Нижний LinearAccelerationDecayFactor
позволяет оценке ориентации быстрее найти «вниз». Нижний MagneticDisturbanceDecayFactor
позволяет оценке ориентации найти север быстрее.
Очень большие, короткие магнитные нарушения порядка отклоняются почти полностью ahrsfilter
. Рассмотрим импульс [0 250 0] uT, приложенный во время записи со стационарного датчика. В идеале не должно быть никаких изменений в оценке ориентации.
ld = load('magJamming.mat'); hpulse = ahrsfilter('SampleRate', ld.Fs); len = 1:10000; qpulse = hpulse(ld.sensorData.Acceleration(len,:), ... ld.sensorData.AngularVelocity(len,:), ... ld.sensorData.MagneticField(len,:)); figure; timevec = 0:ld.Fs:(ld.Fs*numel(qpulse) - 1); plot( timevec, eulerd(qpulse, 'ZYX', 'frame') ); title(['Stationary Trajectory Orientation Euler Angles' newline ... 'Magnetic Jamming Response']); legend('Z-rotation', 'Y-rotation', 'X-rotation'); ylabel('Degrees'); xlabel('Seconds');
Обратите внимание, что фильтр почти полностью отклоняет этот магнитный импульс как интерференцию. Любая напряженность магнитного поля, больше, чем в четыре раза больше ExpectedMagneticFieldStrength
рассматривается как источник помех, и сигнал магнитометра игнорируется для этих выборок.
Заключение
Представленные здесь алгоритмы при правильной настройке позволяют оценить ориентацию и устойчивы к источникам экологического шума. Важно учитывать ситуации, в которых используются датчики, и соответствующим образом настраивать фильтры.
ahrsfilter
| ecompass
| imufilter
| imuSensor
| insfilter