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

Ориентация

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

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

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

Количество ориентации, вычисленное в Sensor Fusion and Tracking Toolbox, является вращением, которое берет количества от родительского ссылочного кадра до дочернего ссылочного кадра. Вращение представлено матрицей вращения или кватернионом.

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

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

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

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

ld = load('rpy_9axis.mat');

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

viewer = HelperOrientationViewer;

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

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

qe = ecompass(acc, mag);
for ii=1:size(acc,1)
    viewer(qe(ii));
    pause(0.01);
end

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

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

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

ifilt = imufilter('SampleRate', ld.Fs);
for ii=1:size(acc,1)
    qimu = ifilt(acc(ii,:), gyro(ii,:));
    viewer(qimu);
    pause(0.01);
end

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

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

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

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

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

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

Заключение

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