AHRS

Ориентация от показаний акселерометра, гироскопа и магнитометра

  • Библиотека:
  • Navigation Toolbox Toolbox/Мультисенсорное позиционирование/Навигационные фильтры

    Sensor Fusion and Tracking Toolbox/Мультисенсорные позиционные/навигационные фильтры

  • AHRS block

Описание

The AHRS Simulink® блок использует акселерометр, магнитометр и данные гироскопического датчика для оценки ориентации устройства.

Порты

Вход

расширить все

Показания акселерометра в системе координат тела датчика в м/с2, заданный как N-на-3 матрица действительных скаляров. N - количество выборок и три столбца Accel представляют измерения [x y z], соответственно.

Типы данных: single | double

Показания гироскопа в системе координат тела датчика в рад/с, заданные как матрица N-на-3 действительных скаляров. N - количество выборок и три столбца Gyro представляют измерения [x y z], соответственно.

Типы данных: single | double

Показания магнитометра в системе координат тела датчика в мкТ, заданные как N-на-3 матрица действительных скаляров. N - количество выборок и три столбца magReadings представляют измерения [x y z], соответственно.

Типы данных: single | double

Выход

расширить все

Ориентация датчика каркаса кузова относительно навигационной системы координат, возвращается как M-на-4 массив скаляров или 3х3-бай- M массив матриц вращения. Каждая строка массива N -by-4 принята как четыре элементов массиваquaternion (Sensor Fusion and Tracking Toolbox). Количество входных выборок, N и параметр Decimation Factor определяют выходной M размера.

Типы данных: single | double

Скорость вращения с смещением гироскопа, удаленным в системе координат тела датчика в рад/с, возвращенная как M-на-3 массив действительных скаляров. Количество входных выборок, N и параметр Decimation Factor определяют выходной M размера.

Типы данных: single | double

Параметры

расширить все

Главный

Навигационная система координат, заданная как NED (Северо-Восток-Даун) или ENU (Восток-Север-Вверх).

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

Количество строк входов - Accel, Gyro и Mag - должно быть кратным коэффициенту десятикратного уменьшения.

Типы данных: single | double

Начальный шум процесса, заданный как матрица 12 на 12 действительных скаляров. Значение по умолчанию, ahrsfilter.defaultProcessNoise, является диагональной матрицей 12 на 12 как:

  Columns 1 through 6

   0.000006092348396                   0                   0                   0                   0                   0
                   0   0.000006092348396                   0                   0                   0                   0
                   0                   0   0.000006092348396                   0                   0                   0
                   0                   0                   0   0.000076154354947                   0                   0
                   0                   0                   0                   0   0.000076154354947                   0
                   0                   0                   0                   0                   0   0.000076154354947
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0

  Columns 7 through 12

                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
                   0                   0                   0                   0                   0                   0
   0.009623610000000                   0                   0                   0                   0                   0
                   0   0.009623610000000                   0                   0                   0                   0
                   0                   0   0.009623610000000                   0                   0                   0
                   0                   0                   0   0.600000000000000                   0                   0
                   0                   0                   0                   0   0.600000000000000                   0
                   0                   0                   0                   0                   0   0.600000000000000

Типы данных: single | double

Выход выходной ориентации, заданный как 'quaternion' или 'Rotation matrix':

  • 'quaternion' - Выход является M -на-4 массивом действительных скаляров. Каждая строка массива представляет четыре компонента quaternion (Sensor Fusion and Tracking Toolbox).

  • 'Rotation matrix' -- Выход является матрицей вращения M 3 на 3 байта.

Выходной размер M зависит от входного размера N и параметра Decimation Factor.

Типы данных: char | string

  • Interpreted execution - Симулируйте модель с помощью MATLAB® интерпретатор. Эта опция сокращает время запуска. В Interpreted execution mode, можно отлаживать исходный код блока.

  • Code generation - Симулируйте модель с помощью сгенерированного кода C. При первом запуске симуляции Simulink генерирует код С для блока. Код С повторно используется для последующих симуляций, пока модель не меняется. Эта опция требует дополнительного времени запуска.

Измерительный шум

Отклонение шума сигнала акселерометра в (м/с2)2, заданный как положительный действительный скаляр.

Типы данных: single | double

Отклонение шума сигнала гироскопа в (рад/с)2, заданный как положительный действительный скаляр.

Типы данных: single | double

Отклонение шума сигнала магнитометра в мкТ2, заданный как положительный действительный скаляр.

Типы данных: single | double

Отклонение дрейфа смещения гироскопа в (рад/с)2, заданный как положительный действительный скаляр.

Типы данных: single | double

Экологический шум

Отклонение линейного шума ускорения в (м/с2)2, заданный как положительный действительный скаляр. Линейное ускорение моделируется как процесс белого шума с фильтрацией методом lowpass.

Типы данных: single | double

Отклонение магнитного нарушения порядка шума в мкТ2, заданный как действительная конечная положительная скалярная величина.

Типы данных: single | double

Коэффициент распада для дрейфа линейного ускорения, заданный как скаляр в области значений [0,1). Если линейное ускорение изменяется быстро, установите этот параметр на более низкое значение. Если линейное ускорение изменяется медленно, установите этот параметр на более высокое значение. Дрейф линейного ускорения моделируется как процесс белого шума с фильтрацией методом lowpass.

Типы данных: single | double

Коэффициент распада для магнитных нарушений порядка, заданный как положительная скалярная величина в область значений [0,1]. Магнитное нарушение порядка моделируется как марковский процесс первого порядка.

Типы данных: single | double

Напряженность магнитного поля в мкТ, заданная как действительная положительная скалярная величина. Напряженность магнитного поля является оценкой напряженности магнитного поля Земли в текущем месте.

Типы данных: single | double

Алгоритмы

расширить все

Примечание: Следующий алгоритм применяется только к опорной системе координат NED.

Блок AHRS использует девятиосевую структуру фильтра Калмана, описанную в [1] (Sensor Fusion and Tracking Toolbox). Алгоритм пытается отследить ошибки в ориентации, смещении гироскопа, линейном ускорении и магнитном нарушении порядка, чтобы вывести конечную ориентацию и скорость вращения. Вместо непосредственного отслеживания ориентации косвенный фильтр Калмана моделирует процесс ошибки, x, с рекурсивным обновлением:

xk=[θkbkakdk]=Fk[θk1bk1ak1dk1]+wk

где x k является вектором 12 на 1, состоящим из:

  • θk -- вектор ошибки ориентации 3 на 1, в степенях, в момент времени k

  • bk -- гироскоп 3 на 1 нуле вектор смещения угловой скорости, в град/с, в момент времени k

  • ak -- вектор ошибки ускорения 3 на 1, измеренный в системе координат датчика, в g, в момент времени k

  • dk -- 3 на 1 вектор ошибки магнитного нарушения порядка, измеренный в системе координат датчика, в мкТ, в момент времени k

и где w k является аддитивным вектором шума 12 на 1, и F k является моделью перехода состояния.

Поскольку x k задан как процесс ошибки, априорная оценка всегда равна нулю, и, следовательно, модель перехода состояния, F k, равна нулю. Это понимание приводит к следующему уменьшению стандартных уравнений Калмана:

Стандартные уравнения Калмана:

xk=Fkxk1+Pk=FkPk1+FkT+Qkyk=zkHkxkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=xk+KkykPk+=PkKkHkPk

Уравнения Калмана, используемые в этом алгоритме:

xk=0Pk=Qkyk=zkSk=Rk+HkPkHkTKk=PkHkT(Sk)1xk+=KkykPk+=PkKkHkPk

где:

  • xk - прогнозируемая (априори) оценка состояния; процесс ошибки

  • Pk - предсказанная (априорная) оценка ковариации

  • yk -- инновации

  • Sk -- инновационная ковариация

  • Kk -- коэффициент усиления Калмана

  • xk+ - обновленная (апостериорная) оценка состояния

  • Pk+ - обновленная (a posteriori) оценка ковариации

k представляет итерацию, надстрочный индекс + представляет апостериорную оценку и надстрочный индекс представляет априорную оценку.

Графические и последующие шаги описывают итерацию на основе одного кадра через алгоритм.

Перед первой итерацией accelReadings, gyroReadings, и magReadings входы шунтируются в DecimationFactor-by-3 системы координат. Для каждого фрагмента алгоритм использует самые актуальные показания акселерометра и магнитометра, соответствующие фрагменту показаний гироскопа.

Ссылки

[1] Комплексирование датчиков с открытым исходным кодом. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., H.J. Luinge, C.T.M. Бэтен и П.Х. Велтинк. Компенсация магнитных нарушений порядка улучшает инерционное и магнитное измерение ориентации сегмента тела человека. Транзакции IEEE по нейронным системам и реабилитационной инженерии. Том 13. Выпуск 3, 2005, с. 395-405.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ Simulink ®

.
Введенный в R2020a
Для просмотра документации необходимо авторизоваться на сайте