Этот пример показывает, как использовать 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 (Sensor Fusion and Tracking Toolbox), могут использоваться, чтобы сглаживать движение.
Следующие объекты оценивают ориентацию, используя либо фильтр Калмана состояния ошибки, либо дополнительный фильтр. Фильтр Калмана состояния ошибки является стандартным фильтром оценки и позволяет настраивать многие различные аспекты системы с использованием соответствующих параметров шума. Дополнительный фильтр может использоваться в качестве замены для систем с ограничениями памяти и имеет минимальные настраиваемые параметры, что позволяет легче настроить за счет более тонкой настройки.
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 рассматривается как источник помех, и сигнал магнитометра игнорируется для этих выборок.
Представленные здесь алгоритмы при правильной настройке позволяют оценить ориентацию и устойчивы к источникам экологического шума. Важно учитывать ситуации, в которых используются датчики, и соответствующим образом настраивать фильтры.