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

Ориентация

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

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

Если задано, следующие алгоритмы могут оценить ориентацию относительно системы координаты вышестоящего элемента "восточного севера" (ENU) вместо NED.

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

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

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

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

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

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

ld = load('rpy_9axis.mat');

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

pp = poseplot;

Fusion магнитометра акселерометра

ecompass функционируйте акселерометр предохранителей и данные о магнитометре. Это - алгоритм без памяти, который не требует никакой настройки параметра, но алгоритм очень восприимчив к шуму датчика.

qe = ecompass(acc, mag);
for ii=1:size(acc,1)
    set(pp, "Orientation", qe(ii))
    drawnow limitrate
end

Обратите внимание на то, что ecompass алгоритм правильно находит местоположение севера. Однако, потому что функция без памяти, предполагаемое движение не является гладким. Алгоритм мог использоваться в качестве шага инициализации в фильтре ориентации или некоторые методы, представленные в Ориентации Фильтра Lowpass Используя Кватернион, SLERP мог использоваться, чтобы сглаживать движение.

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

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

imufilter и complementaryFilter Система objects™ плавит данные о гироскопе и акселерометр. imufilter использует внутренний Фильтр Калмана состояния ошибки и complementaryFilter использует дополнительный фильтр. Фильтры способны к удалению шума смещения гироскопа, который дрейфует в зависимости от времени.

ifilt = imufilter('SampleRate', ld.Fs);
for ii=1:size(acc,1)
    qimu = ifilt(acc(ii,:), gyro(ii,:));
    set(pp, "Orientation", qimu)
    drawnow limitrate
end

% Disable magnetometer input.
cfilt = complementaryFilter('SampleRate', ld.Fs, 'HasMagnetometer', false);
for ii=1:size(acc,1)
    qimu = cfilt(acc(ii,:), gyro(ii,:));
    set(pp, "Orientation", qimu)
    drawnow limitrate
end

Несмотря на то, что imufilter и complementaryFilter алгоритмы производят значительно более сглаженные оценки движения, по сравнению с ecompass, они правильно не оценивают направление севера. imufilter не обрабатывает данные о магнитометре, таким образом, они просто принимают, что Ось X устройства первоначально указывает к северу. Оценка движения дана imufilter относительно оцененной ориентации начальной буквы. complementaryFilter делает то же предположение когда HasMagnetometer свойство установлено в false.

Fusion магнитометра гироскопа акселерометра

И заголовок ссылочной системы (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,:));
    set(pp, "Orientation", qahrs)
    drawnow limitrate
end

cfilt = complementaryFilter('SampleRate', ld.Fs);
for ii=1:size(acc,1)
    qahrs = cfilt(acc(ii,:), gyro(ii,:), mag(ii,:));
    set(pp, "Orientation", qahrs)
    drawnow limitrate
end

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

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

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

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

LinearAccelerationNoise и LinearAccelerationDecayFactor управляйте ответом фильтра на линейное (поступательное) ускорение. Сотрясение устройства является простым примером добавления линейного ускорения.

Рассмотрите как imufilter с LinearAccelerationNoise из 9e-3 $$(m/s^2{)}^2$$ отвечает на дрожащую траекторию, по сравнению с одной с LinearAccelerationNoise из 9e-4 $$(m/s^2{)}^2$$.

ld = load('shakingDevice.mat');
accel = ld.sensorData.Acceleration;
gyro = ld.sensorData.AngularVelocity;

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

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

Заключение

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