Ориентация

Ориентация объекта описывает его поворот относительно некоторой системы координат, иногда называемой родительской системой координат, в трёх размерностях.

Для следующих алгоритмов используется фиксированная, родительская система координат 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 рассматривается как источник помех, и сигнал магнитометра игнорируется для этих выборок.

Заключение

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