Документация

Ориентация

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

Для следующих алгоритмов используется фиксированная родительская система координат «север-восток-вниз» (NED). NED иногда называют глобальной системой координат или опорной системой координат. В системе координат NED ось X указывает на север, ось Y указывает на восток, а ось Z указывает вниз. Плоскость X-Y NED считается локальной касательной плоскостью Земли. В зависимости от алгоритма север может быть либо магнитным северным, либо истинным северным.

Если указано, следующие алгоритмы могут оценивать ориентацию относительно родительской системы координат Восток-Север-Вверх (RUS) вместо NED.

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

Вычисленная величина ориентации - это поворот, который берет величины от родительского опорного кадра к дочернему опорному кадру. Вращение представлено матрицей кватерниона или вращения.

Типы датчиков

Для оценки ориентации обычно используются три типа датчиков: акселерометры, гироскопы и магнитометры. Акселерометры измеряют правильное ускорение. Гироскопы измеряют угловую скорость. Магнитометры измеряют локальное магнитное поле. Различные алгоритмы используются для сплавления различных комбинаций датчиков для оценки ориентации.

Данные датчика

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

ld = load('rpy_9axis.mat');

acc = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;
mag = ld.sensorData.MagneticField;

viewer = HelperOrientationViewer;

Синтез акселерометра-магнитометра

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, могут быть использованы для сглаживания движения.

Акселерометр-гироскоп Fusion

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

imufilter и complementaryFilter Система objects™ данных акселерометра предохранителя и гироскопа. 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 относительно первоначальной расчетной ориентации. complementaryFilter делает то же самое предположение, когда HasMagnetometer свойство имеет значение false.

Акселерометр-гироскоп-магнитометрический синтез

Опорная система ориентации и курса (AHRS) состоит из 9-осевой системы, которая использует акселерометр, гироскоп и магнитометр для вычисления ориентации. 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

Настройка параметров фильтра

complementaryFilter, imufilter, и ahrsfilter Все системные objects™ имеют настраиваемые параметры. Настройка параметров на основе заданных используемых датчиков может повысить производительность.

complementaryFilter параметры AccelerometerGain и MagnetometerGain можно настроить на изменение величины влияния измерений каждого датчика на оценку ориентации. Когда AccelerometerGain имеет значение 0, только гироскоп используется для ориентации по оси x и y. Когда AccelerometerGain имеет значение 1, для ориентации по осям X и Y используется только акселерометр. Когда MagnetometerGain имеет значение 0, только гироскоп используется для ориентации оси Z. Когда MagnetometerGain имеет значение 1для ориентации оси Z используется только магнитометр.

ahrsfilter и imufilter Системные objects™ имеют больше параметров, которые позволяют фильтрам более точно соответствовать определенным аппаратным датчикам. Также важно учитывать окружающую среду датчика. imufilter параметры являются подмножеством ahrsfilter параметры. AccelerometerNoise, GyroscopeNoise, MagnetometerNoise, и GyroscopeDriftNoise являются измерительными шумами. Таблицы данных датчиков помогают определить эти значения.

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');

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

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

Заключение

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