Устройство для определения положения объекта в пространстве - RU2626755C1

Чертежи

Описание

ОБЛАСТЬ ТЕХНИКИ

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

УРОВЕНЬ ТЕХНИКИ

Известно Устройство для определения положения объекта в пространстве (патент RU 2542793, Чернышов Е.С., G01C 21/08, опубл. 27.02.2015). Данное изобретение по совокупности существенных признаков наиболее близко предлагаемому и принято за прототип. Известное устройство состоит из катушки индуктивности (источник поля), управляемого стабилизатора тока, цифрового трехкомпонентного магнитометра, цифрового трехкомпонентного акселерометра и вычислительного блока. Известное устройство определяет положение объекта за счет измерения акселерометром вектора ускорения свободного падения, а магнитометром - вектора индукции магнитного поля Земли и вектора индукции магнитного поля катушки индуктивности, расположенной неподвижно в горизонтальной плоскости.

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

РАСКРЫТИЕ ИЗОБРЕТЕНИЯ

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

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

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

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

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

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

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

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

В другом частном варианте осуществления выход магнитометра подключен ко входу блока выделения частотных компонент магнитного поля.

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

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

В другом частном варианте осуществления выход гироскопа подключен ко второму входу блока вычисления углов ориентации.

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

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

КРАТКОЕ ОПИСАНИЕ ЧЕРТЕЖЕЙ

Фиг. 1 - Структурная схема устройства для определения положения объекта в пространстве.

Фиг. 2 - График зависимости тока катушки индуктивности от времени.

Фиг. 3 - Топология системы, содержащей несколько источников поля и несколько независимых позиционируемых объектов.

ОСУЩЕСТВЛЕНИЕ ИЗОБРЕТЕНИЯ

Предлагаемое устройство состоит из катушки индуктивности 1 (источник поля), размещенной неподвижно в горизонтальной плоскости, и подключенной к управляемому стабилизатору тока 2, который с заданной периодичностью получает сигнал смены полярности тока от тактового генератора 3. Управляемый стабилизатор тока 2 вместе с тактовым генератором 3 образуют источник питания катушки индуктивности 4.

Цифровые трехкомпонентные магнитометр 5, гироскоп 6 и акселерометр 7 расположены на платформе 8. Платформа с датчиками 8 жестко закреплена на подвижном объекте. Магнитометр 5, гироскоп 6 и акселерометр 7 взаимодействуют с вычислительным блоком 14, который содержит: блок выделения частотных компонент магнитного поля 9; блок фазовой автоподстройки частоты 10; блок вычисления углов ориентации платформы с датчиками 11; блок компенсации поворота платформы с датчиками 12; блок вычисления координат платформы с датчиками 13. Причем выход магнитометра 5 подключен ко входу блока выделения частотных компонент магнитного поля 9, первый выход которого подключен ко входу блока фазовой автоподстройки частоты 10, второй выход подключен к первому входу блока компенсации поворота 12, третий выход подключен к блоку вычисления углов ориентации 11, выход блока фазовой автоподстройки частоты 10 подключен ко входу магнитометра 5, выход гироскопа 6 подключен ко второму входу блока вычисления углов ориентации 11, выход акселерометра 7 подключен к третьему входу блока вычисления углов ориентации 11, выход блока вычисления углов ориентации 11 подключен ко второму входу блока компенсации поворота 12, выход которого подключен ко входу блока вычисления координат 13.

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

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

Под рабочей областью подразумевается область пространства вокруг катушки индуктивности 1, в которой осуществляется позиционирование платформы с датчиками 8. Размер рабочей области зависит от характеристик катушки индуктивности 1, источника питания 4 и магнитометра 5, а также от требуемой точности измерений. В первом приближении погрешность измерений пропорциональна квадрату расстояния от магнитометра до катушки индуктивности [1], следовательно, для обеспечения требуемой точности измерений, необходимо ограничивать размер рабочей области устройства. При этом за пределами рабочей области точность измерений по одной или нескольким координатам будет ниже требуемой величины. Дальнейшее увеличение расстояния ведет к падению величины индукции магнитного поля, измеряемой магнитометром, ниже уровня чувствительности/погрешности магнитометра, что делает невозможным определение координат платформы с датчиками.

Важной характеристикой также является постоянная времени катушки индуктивности 1, так как от нее зависит длительность переключения полярности тока катушки, которая, в свою очередь, влияет на максимально достижимую частоту измерений устройства. Для снижения длительности переключения полярности необходимо уменьшать постоянную времени, т.е. уменьшать индуктивность и/или увеличивать активное электрическое сопротивление катушки.

Управляющая часть стабилизатора тока 2 и тактовый генератор 3 могут быть собраны на дискретных электронных компонентах по известным схемам, или реализованы с помощью микроконтроллера. Во втором случае тактовый генератор будет представлять собой подпрограмму данного микроконтроллера.

Магнитометр 5 может быть представлен трехкомпонентным датчиком индукции магнитного поля любого известного типа (магниторезистивный датчик, датчик на эффекте Холла), выполненным в виде микросхемы с цифровым выходом, по технологии МЭМС (микроэлектромеханические системы), или в виде сборки, включающей три ортогональных чувствительных элемента, а также схему усиления и аналого-цифрового преобразования сигнала. Обязательным условием является наличие у магнитометра режима одиночных измерений с внешним сигналом начала измерения. Подходящими моделями магнитометров являются STMicroelectronics LIS3MDL, MEMSIC MMC3416xPJ, и аналогичные им.

Гироскоп 6 и акселерометр 7 представляют собой трехкомпонентные датчики, выполненные по технологии МЭМС, в виде микросхемы с цифровым выходом.

С точки зрения себестоимости и массогабаритных показателей, наиболее предпочтительным вариантом является использование комбинированного датчика, выполненного в виде единой цифровой МЭМС микросхемы, включающей трехкомпонентный магнитометр, трехкомпонентный гироскоп и трехкомпонентный акселерометр. Таким датчиком, например, является STMicroelectronics LSM9DS1.

Платформа с датчиками 8 представляет собой печатную плату, на которой расположены магнитометр 5, гироскоп 6 и акселерометр 7. При использовании цифровых датчиков, выполненных по технологии МЭМС, платформа 8 может иметь размеры менее 15×15×3 мм.

Вычислительный блок 14 может быть представлен микроконтроллером или персональным/одноплатным компьютером, при этом внутренние блоки 9, 10, 11, 12, 13 вычислительного блока представляют собой подпрограммы. Вычислительный блок реализует алгоритм определения координат объекта исходя из информации о величине вектора искусственного магнитного поля в данной точке пространства, совместно с алгоритмом определения ориентации объекта.

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

Предлагаемое устройство работает следующим образом.

Управляемый стабилизатор тока 2 подает на катушку 1 токовый сигнал i(t) поочередно положительной и отрицательной полярности (фиг. 2). Полярность сигнала i(t) задается периодическим управляющим сигналом, формируемым тактовым генератором 3. Ток, проходящий через катушку 1, создает магнитное поле. Искусственное магнитное поле катушки 1 складывается с магнитным полем Земли и магнитометр 5 производит измерение вектора магнитной индукции результирующего поля. Одновременно с этим гироскоп 6 производит измерение угловой скорости платформы с датчиками, акселерометр 7 производит измерение вектора ускорения свободного падения, действующего на платформу с датчиками.

Для корректной работы блока выделения частотных компонент магнитного поля 9 и блока фазовой автоподстройки частоты 10, максимальная частота измерений ƒ_mag магнитометра 5 должна удовлетворять следующему условию:

где T₀ - период сигнала катушки индуктивности 1 (см. фиг. 2).

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

Показания датчиков 5, 6 и 7 в цифровом виде передаются на вычислительный блок 14.

Для простоты принято, что системы координат датчиков 5, 6 и 7 совпадают с системой координат, связанной с подвижной платформой 8. В ином случае показания датчиков должны приводиться в вычислительном блоке 14 к единой системе координат, связанной с подвижной платформой.

Для определения координат и ориентации платформы с датчиками 8 вычислительный блок выполняет следующие операции.

Блок выделения частотных компонент магнитного поля 9 производит накопление массива измерений по каждой оси координат магнитометра 5 за целое число периодов сигнала катушки индуктивности 1 (не менее чем за один период), после чего осуществляет выделение постоянной составляющей магнитного поля - вектора индукции магнитного поля Земли

, а также переменной составляющей магнитного поля - вектора индукции магнитного поля катушки индуктивности . Для этой цели используется преобразование Фурье, в котором базисом выступают функции Уолша [2]. Могут быть использованы и другие способы преобразования.

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

При этом фаза компоненты характеризует ее знак следующим образом: если фазы двух измеряемых компонент совпадают, то компоненты имеют одинаковый знак; если фазы различаются на π (половина периода), то компоненты имеют противоположные знаки; скачок фазы на π соответствует изменению знака компоненты. Таким образом, вектор

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

Неопределенность фазы может быть устранена несколькими способами.

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

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

Значение фазы каждой из трех компонент переменной составляющей магнитного поля далее передается в блок фазовой автоподстройки частоты 10, в котором выполняется алгоритм минимизации ошибки между заданной и измеренной фазой сигнала путем корректировки частоты измерений магнитометра (например, алгоритм ПИД-регулирования).

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

В результате работы блока 10 частота и фаза измерений магнитометра 5 будут соответствовать частоте и фазе сигнала катушки индуктивности 1.

На следующем этапе вычислений величина вектора индукции магнитного поля Земли

передается из блока 9 в блок вычисления углов ориентации 11, куда также поступают измерения гироскопа 6 и акселерометра 7.

Измерения акселерометра 7 представляют собой вектор ускорения свободного падения

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

Используем матрицы поворота [3] для вращения векторов на углы, полученные интегрированием измерений гироскопа. Пусть период измерений гироскопа и период измерения векторов

и равны T₀. Тогда рекурсивный комплементарный фильтр [4] будет иметь следующий вид:

где

- отфильтрованный вектор индукции магнитного поля Земли; δ - коэффициент фильтра, δ<<1; W_x, W_y, W_z - измеренные скорости вращения гироскопа вокруг осей x, y, z соответственно; Т₀ - период измерений гироскопа; R_x(W_xT₀) - матрица поворота вокруг оси x на угол W_xT₀; R_y(W_yT₀) - матрица поворота вокруг оси y на угол W_yT₀; R_z(W_zT₀) - матрица поворота вокруг оси z на угол W_zT₀; - отфильтрованный вектор ускорения свободного падения.

Вычисление углов ориентации производится известным способом [3], по направлению векторов

и .

Далее полученные углы ориентации объекта используются в блоке компенсации поворота 12, для перевода вектора индукции магнитного поля катушки индуктивности

из системы координат магнитометра 5 в систему координат, связанную с неподвижной катушкой индуктивности 1. Для этой цели производится поворот вектора на углы, обратные найденным углам ориентации объекта. Поворот вектора осуществляется с помощью матриц поворота:

где

- вектор индукции магнитного поля катушки индуктивности в системе координат, связанной с катушкой индуктивности 1 (при φ=θ=ψ=0); φ, θ, ψ - найденные углы ориентации объекта (углы крена, тангажа и рыскания соответственно); R_z(-ψ) - матрица поворота вокруг оси z на угол -ψ; R_y(-θ) - матрица поворота вокруг оси y на угол -θ; R_x(-φ) - матрица поворота вокруг оси x на угол -φ.

Вектор

затем передается в блок вычисления координат 13. Пусть вектор имеет следующий вид:

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

в сферической системе координат (угол между проекцией вектора на плоскость x0y и осью x системы координат катушки индуктивности 1) совпадает с азимутальной координатой ϕ позиционируемого объекта:

Оставшиеся координаты (радиус-вектор и зенитный угол) объекта могут быть найдены аналитически [1], или с помощью таблично заданной функции [3]. Во втором случае таблица представляет собой массив точек вертикальной плоскости, проходящей через вертикальную ось z системы координат катушки индуктивности 1 (ось симметрии катушки). Каждой точке сопоставлен вектор магнитной индукции, создаваемый катушкой индуктивности в данной точке. Расчет векторов может быть произведен с помощью закона Био-Савара-Лапласа. Для определения координат объекта необходимо найти в таблице векторы индукции магнитного поля, наиболее близкие по модулю и направлению к вектору

, и значения их координат. После чего можно определить искомые координаты объекта, например, с помощью билинейной интерполяции [5].

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

Современный уровень развития вычислительной техники позволяет использовать отдельный миниатюрный вычислительный блок (поз. 25, 26, 27) для каждой платформы с датчиками (поз. 22, 23, 24). Такой подход, вместе с отсутствием линии связи между вычислительными блоками и источником питания катушки индуктивности, позволяет неограниченно наращивать количество позиционируемых объектов, без внесения каких-либо изменений в структуру устройства.

Для расширения рабочей области устройства может быть использовано несколько катушек индуктивности (поз. 15, 16, 17), расположенных раздельно. При этом сигналы катушек должны иметь частотное или временное разделение, для предотвращения их смешивания на границе между рабочими областями нескольких катушек. Выделение сигнала той или иной катушки может быть осуществлено с помощью преобразования Фурье с базисом в виде функций Уолша [2], или другими способами.

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

Наиболее подходящим способом достижения указанной цели является подключение входов управляемых стабилизаторов тока катушек (поз. 18, 19, 20) к выходам одного общего тактового генератора 21, обеспечивающего генерацию сигналов требуемой частоты и фазы.

В общем случае, для создания рабочей области произвольных размеров и формы требуется чередование источников поля с не менее чем четырьмя различными частотами (при частотном разделении) или фазами (при временном разделении) сигнала [6]. Однако если допустимо наличие "мертвых" зон, то также возможно чередование источников поля с двумя или тремя различными частотами/фазами.

Используемые обозначения на чертежах:

поз. 1 - катушка индуктивности;

поз. 2 - управляемый стабилизатор тока;

поз. 3 - тактовый генератор;

поз. 4 - источник питания;

поз. 5 - магнитометр;

поз. 6 - гироскоп;

поз. 7 - акселерометр;

поз. 8 - платформа с датчиками;

поз. 9 - блок выделения частотных компонент магнитного поля;

поз. 10 - блок фазовой автоподстройки частоты;

поз. 11 - блок вычисления углов ориентации;

поз. 12 - блок компенсации поворота;

поз. 13 - блок вычисления координат;

поз. 14 - вычислительный блок;

поз. 15 - катушка индуктивности 1;

поз. 16 - катушка индуктивности 2;

поз. 17 - катушка индуктивности N;

поз. 18 - управляемый стабилизатор тока 1;

поз. 19 - управляемый стабилизатор тока 2;

поз. 20 - управляемый стабилизатор тока N;

поз. 21 - тактовый генератор;

поз. 22 - платформа с датчиками 1;

поз. 23 - платформа с датчиками 2;

поз. 24 - платформа с датчиками М;

поз. 25 - вычислительный блок 1;

поз. 26 - вычислительный блок 2;

поз. 27 - вычислительный блок М.

Источники информации:

1. Raab F., Blood Е., Steiner О., Jones Н. Magnetic position and orientation tracking system // IEEE Transactions on Aerospace and Electronics Systems. - Vol. AES-15, No. 5, September 1979. - P. 709-717.

2. Залманзон Л.А. Преобразования Фурье, Уолша, Хаара и их применение в управлении, связи и других областях. - М.: Наука. Гл. ред. физ.-мат. лит., 1989. - 496 с.

3. Чье Е.У., Чернышов Е.С. Магнитно-инерциальный метод определения положения и ориентации объекта // Вестник Тихоокеанского государственного университета. - 2014. - №1 (32). - С. 69-78.

4. Чернышов Е.С., Отческий С.А. Микроэлектромеханические системы отслеживания перемещений // Актуальные вопросы в научной работе и образовательной деятельности: сборник научных трудов по материалам Международной научно-практической конференции 31 января 2013 г.: в 13 частях. Часть 8. - Тамбов: Изд-во ТРОО "Бизнес-Наука-Общество", 2013. - С. 147-148.

5. Ярославский Л.П. Введение в цифровую обработку изображений. - М.: Сов. радио, 1979. - 312 с.

6. Желамский М.В. Особенности создания поля позиционирования для локальной навигации в закрытых пространствах // Измерительная техника. - 2014. - №7. - С. 40-45; Zhelamskii M.V. Features of the construction of a positioning field for local navigation in enclosed spaces // Measurement Techniques. - 2014. - Vol. 57. - No. 7. P. 791-799.

Реферат

Изобретение относится к измерительной технике и может быть использовано для создания средств измерения координат и угловых величин объекта. Устройство для определения положения объекта в пространстве содержит катушку индуктивности, размещенную неподвижно в горизонтальной плоскости, источник питания катушки индуктивности, вычислительный блок, а также платформу с датчиками, размещенную на подвижном объекте, включающую трехкомпонентный магнитометр, трехкомпонентный акселерометр и трехкомпонентный гироскоп, причем источник питания катушки индуктивности включает в себя стабилизатор тока катушки, управляемый тактовым генератором, а вычислительный блок выполнен с возможностью фазовой автоподстройки частоты, обеспечивающей тактирование магнитометра и синхронизацию фазы измерений магнитометра с фазой импульсов магнитного поля, генерируемых катушкой индуктивности. Технический результат – повышение точности измерения. 8 з.п. ф-лы, 3 ил.

Формула