close

Вход

Забыли?

вход по аккаунту

Служба в органах Следственного комитета;pdf

код для вставкиСкачать
ИНФОРМАТИКА, ВЫЧИСЛИТЕЛЬНАЯ
ТЕХНИКА И УПРАВЛЕНИЕ
ВЕСТНИК ТОГУ. 2014. № 1(32)
УДК 51-74:[621.317.49+681.586]
© Чье Ен Ун, Е. С. Чернышов, 2014
МАГНИТНО-ИНЕРЦИАЛЬНЫЙ МЕТОД ОПРЕДЕЛЕНИЯ
ПОЛОЖЕНИЯ И ОРИЕНТАЦИИ ОБЪЕКТА
Чье Ен Ун – д-р техн. наук, проф., зав. кафедрой «Автоматика и системотехника»,
e-mail: [email protected]; Чернышов Е. С. – асп. кафедры «Автоматика и системотехника», e-mail: [email protected] (ТОГУ)
В статье описан гибридный магнитно-инерциальный метод определения
положения и ориентации объекта, использующий одну катушку индуктивности и современные цифровые трехосевые МЭМС датчики - магнитометр
и акселерометр. Приведены алгоритмы для вычисления углов поворота и
координат датчика. Предложен способ преобразования вектора магнитной
индукции в координаты датчика с помощью таблично заданной функции.
Ключевые слова: позиционирование, положение и ориентация объекта, индукция магнитного поля, магнитометр, акселерометр, закон Био-Савара-Лапласа,
матрица поворота, сферические координаты.
Введение
Магнитное позиционирование в настоящее время активно применяется в
медицине, тренажерных системах и военной технике, т.к. в отличие от многих других методов обладает достаточной робастностью для данных применений. Известны методы магнитного позиционирования, использующие три
ортогональных катушки индуктивности – источника магнитного поля, и один
трехосевой магнитометр [1], или две ортогональные катушки и комбинацию из нескольких магнитометров, расположенных в вершинах многоугольника [2].
Описанный в статье метод является гибридным, содержащим элементы
как магнитных так и инерциальных методов позиционирования, что позволяет использовать единственную катушку индуктивности и современный цифровой инерциальный измерительный модуль, состоящий из акселерометра и
магнитометра, выполненных по технологии микроэлектромеханических систем (МЭМС).
Теоретические основы
Вектор магнитной индукции является основной величиной, характеризующей магнитное поле в некоторой точке пространства. Именно эта вели69
Чье Ен Ун, Чернышов Е. С.
ВЕСТНИК ТОГУ. 2014 № 1 (32)
чина измеряется датчиками магнитного поля - магнитометрами. Для известного источника поля, магнитную индукцию можно вычислить с помощью
закона Био-Савара-Лапласа [3] (представлен в дискретном виде):
⎡ uur uur
r ⎤
I ⎢ Δli , uri ⎥
uur ur
ri ⎥
uuur μ μ I ⎡ Δli , ri ⎤ μ μ ⎢
dBi = 0 ⎣ ur 3 ⎦ = 0 ⎣ ur 2 ⎦ ,
4π
4π
ri
ri
uuur
где dBi - составляющая вектора магнитной индукции, созданная фрагменuur
том проводника Δli (направление вектора совпадает с направлением тока I
ur
в проводнике); ri - вектор, соединяющий данную точку пространства с
uur
фрагментом проводника Δli ; μ 0 - магнитная постоянная; μ - относительная
магнитная проницаемость среды (для воздуха можно принять равной единице).
Чтобы найти результирующий вектор магнитной индукции в определенной точке пространства, необходимо сложить составляющие магнитной
индукции, созданные всеми фрагментами проводника:
ur
uuur
B = ∑ dBi .
i
Исследуя распределение векторов индукции магнитного поля искусственного источника – катушки индуктивности, можно сделать вывод, что
величина и направление вектора магнитной индукции с определенными
ограничениями и допущениями могут характеризовать координаты соответствующей точки пространства относительно источника [4].
Так как имеет место интегрирование составляющих магнитной индукции по контуру проводника, то выразить координаты точки из закона БиоСавара-Лапласа аналитически не представляется возможным. В [1] описана
функциональная зависимость между вектором магнитной индукции и координатами точки, для идеального источника – магнитного диполя. Т.к. реальные катушки не являются диполями, то данная зависимость с некоторой
погрешностью справедлива только если расстояние до катушки много
больше ее диаметра (не менее четырех диаметров). Известны аналитические
методы, которые учитывают также диаметр катушки, что позволяет значительно снизить погрешность на малых расстояниях от нее [5]. Однако они
имеют достаточно большую вычислительную сложность для выполнения на
микроконтроллерах в реальном времени.
Преградой на пути обратного преобразования измеренного вектора
магнитной индукции в координаты также является то, что, в общем случае
трехмерного позиционирования, датчик имеет произвольную ориентацию
(свободно вращается), и его показания не соответствуют реальному направ70
МАГНИТНО-ИНЕРЦИАЛЬНЫЙ МЕТОД
ОПРЕДЕЛЕНИЯ ПОЛОЖЕНИЯ
И ОРИЕНТАЦИИ ОБЪЕКТА
ВЕСТНИК ТОГУ. 2014. № 1(32)
лению вектора магнитной индукции (здесь и далее предполагается, что датчик имеет три ортогональных оси измерений). Таким образом, для определения координат датчика с помощью единственной катушки индуктивности, необходимо знать его ориентацию. Для этой цели используется акселерометр в функции инклинометра, и магнитометр в функции геомагнитного
датчика, образующие инерциальный измерительный модуль.
Магнитно-инерциальный метод
Для определения ориентации датчика необходимо иметь информацию о
направлении магнитного поля Земли. Ее можно получить, периодически
отключая катушку индуктивности от питания. При таком подходе частота
измерений уменьшится. Кроме того, катушка с большой индуктивностью
вносит значительную инерционность, ее отключение и последующее включение будут требовать больших (относительно частоты измерений) временных затрат.
Поэтому предлагается переключать полярность катушки перед каждым
измерением. Если сложить два последовательных измерения, то переменная
составляющая магнитного поля компенсируется, и результат будет равен
удвоенному значению индукции магнитного поля Земли. Если вычесть из
текущего измерения предыдущее, то компенсируется постоянная составляющая магнитного поля, и результат будет равен двум величинам магнитного поля катушки в данной точке пространства:
uur uur uur uuur
Bi = B p + Bs + BH ,
uuur uur uur uuur
Bi −1 = B p − Bs + BH ,
uur uuur
uur uuur
Bi + Bi −1 = 2 ⋅ ( B p + BH ),
uur uuur
uur
Bi − Bi −1 = 2 ⋅ Bs ,
uur
uur
где Bi - текущее измерение; Bp - вектор индукции магнитного поля Земли;
uur
Bs - вектор индукции магнитного поля катушки (меняет полярность при пеuuur
реключении); BH - вектор паразитного смещения показаний датчика, измеuuur
ряется на этапе калибровки; Bi −1 - предыдущее измерение.
Смена полярности при мостовом подключении катушки занимает значительно более короткий промежуток времени, чем отключение. Кроме того, частота измерений в данном случае падает незначительно, так как, фактически, вместо пропуска итерации на измерение магнитного поля Земли,
осуществляется экспоненциальное усреднение измерений с коэффициентом
0,5. Дополнительным эффектом от усреднения является снижение шума измерений.
71
ВЕСТНИК ТОГУ. 2014 № 1 (32)
Чье Ен Ун, Чернышов Е. С.
Данный алгоритм справедлив, если предположить, что за два последовательных измерения положение и ориентация датчика изменились незначительно, т.е. если динамика системы значительно ниже частоты измерений.
Определение ориентации
uur
Полученный вектор индукции магнитного поля Земли Bp далее используется для определения ориентации датчика, совместно с показаниями акселерометра. Угловое положение датчика определяется как поворот на определенные углы крена (roll), тангажа (pitch) и рыскания (yaw) вокруг осей координат датчика x, y, z соответственно, относительно начального углового положения датчика.
Выберем правую систему координат (рис. 1). Начальным угловым положением для простоты дальнейших расчетов принято положение датчика, в
котором акселерометр имеет по своей оси z значение величины ускорения
свободного падения g, зависящее от географического положения устройства,
и 0 по осям x и y. А магнитометр имеет 0 по оси y, и некоторые значения
B cos δ и B sin δ по осям x и z, так как вектор индукции магнитного поля
Земли направлен под определенным углом δ к горизонту, зависящим от географического положения (рис. 2). Модуль вектора B также зависит от географического положения устройства.
Рис. 1. Система координат датчиков
Рис. 2. Составляющие вектора индукции магнитного поля Земли
72
МАГНИТНО-ИНЕРЦИАЛЬНЫЙ МЕТОД
ОПРЕДЕЛЕНИЯ ПОЛОЖЕНИЯ
И ОРИЕНТАЦИИ ОБЪЕКТА
ВЕСТНИК ТОГУ. 2014. № 1(32)
ur
Система векторов индукции магнитного поля Земли B и ускорения своur
бодного падения G определяет систему координат устройства (систему координат, связанную с Землей).
Векторы ускорения свободного падения и индукции магнитного поля
Земли, измеренные датчиками в начальном угловом положении, должны
быть следующими:
⎡0⎤
⎡ cos δ ⎤
uur ⎢ ⎥ uur
(1)
G0 = 0 , B0 = B ⎢ 0 ⎥ .
⎢ ⎥
⎢⎣ g ⎥⎦
⎢
⎥
⎢⎣ sin δ ⎥⎦
После поворота на углы крена φ , тангажа θ и рыскания ψ показания
uur uur
датчиков G p и Bp будут описываться следующими уравнениями:
⎡G ⎤
uur ⎢ px ⎥
uur
Gp = ⎢G py ⎥ = Rx (φ ) Ry (θ ) Rz (ψ )G0 ,
⎢Gpz ⎥
⎣ ⎦
(2)
⎡B ⎤
uur ⎢ px ⎥
uur
Bp = ⎢ Bpy ⎥ = Rx (φ ) Ry (θ ) Rz (ψ ) B0 ,
⎢ Bpz ⎥
⎣ ⎦
где Rx (φ ) - матрица поворота вокруг оси x на угол φ ; Ry (θ ) - матрица поворота вокруг оси y на угол θ ; Rz (ψ ) - матрица поворота вокруг оси z на угол
ψ:
0
0 ⎤
⎡1
Rx (φ ) = ⎢⎢0 cos φ sin φ ⎥⎥ ,
⎢⎣0 − sin φ cos φ ⎥⎦
⎡cosθ
Ry (θ ) = ⎢⎢ 0
⎢⎣ sin θ
⎡ cosψ
Rz (ψ ) = ⎢⎢ − sinψ
⎢⎣ 0
0 − sin θ ⎤
1
0 ⎥⎥ ,
0 cosθ ⎥⎦
sinψ
cosψ
0
(3)
0⎤
0 ⎥⎥ .
1 ⎥⎦
Далее используется алгоритм компенсации наклона, в котором по отклонению вектора ускорения свободного падения от начального положения сначала определяются углы крена и тангажа. А затем производится компенсация
этих углов, и определяется угол рыскания по отклонению вектора индукции
магнитного поля Земли от начального положения [6]. Из выражений (2) получим:
73
Чье Ен Ун, Чернышов Е. С.
ВЕСТНИК ТОГУ. 2014 № 1 (32)
⎡G px ⎤
⎡0⎤ ⎡0⎤
⎢
⎥
(4)
Ry (−θ ) Rx (−φ ) ⎢G py ⎥ = Rz (ψ ) ⎢⎢ 0 ⎥⎥ = ⎢⎢ 0 ⎥⎥ .
⎢ G pz ⎥
⎢⎣ g ⎥⎦ ⎢⎣ g ⎥⎦
⎣
⎦
Так как вектор ускорения свободного падения, исходя из выбранного
начального его положения (1), имеет неопределенный угол рыскания ψ (это
uur
поворот вектора G0 вокруг сонаправленной ему оси z), то его поворот матрицей Rz (ψ ) оставит вектор неизменным.
Подставляем в выражение (4) матрицы поворота (3):
⎡ cos θ sin θ sin φ sin θ cos φ ⎤ ⎡G px ⎤ ⎡ 0 ⎤
⎢
⎥
⎢ 0
− sin φ ⎥⎥ ⎢G py ⎥ = ⎢⎢ 0 ⎥⎥ .
cos φ
⎢
⎢⎣ − sin θ cosθ sin φ cosθ cos φ ⎥⎦ ⎢⎣ G pz ⎥⎦ ⎢⎣ g ⎥⎦
Решая полученную систему уравнений, находим выражения для углов φ
иθ:
⎛ G py
tan φ = ⎜
⎜ G pz
⎝
⎞
⎟⎟ ,
⎠
⎛
⎞
−G px
tan θ = ⎜
.
⎜ G py sin φ + G pz cos φ ⎟⎟
⎝
⎠
Далее требуется определить угол рыскания ψ датчика.
Из выражений (2) и (3) получим:
⎡ B px ⎤
⎡ B cos δ ⎤
⎢ ⎥
Ry (−θ ) Rx (−φ ) ⎢ B py ⎥ = Rz (ψ ) ⎢⎢ 0 ⎥⎥ .
⎢ B pz ⎥
⎢⎣ B sin δ ⎥⎦
⎣ ⎦
Подставляя в это выражение матрицы поворота (3), находим:
⎡ B px cos θ + B py sin θ sin φ + B pz sin θ cos φ ⎤ ⎡ cosψ B cos δ ⎤
⎢
⎥ ⎢
⎥
B py cos φ − B pz sin φ
⎢
⎥ = ⎢ − sinψ B cos δ ⎥ .
⎢ B px sin θ + B py cos θ sin φ + B pz cos θ cos φ ⎥ ⎢⎣
⎥⎦
B sin δ
⎣
⎦
Далее находим выражение для угла ψ :
⎛
⎞
B pz sin φ − Bpy cos φ
tanψ = ⎜
.
⎜ Bpx cosθ + Bpy sin θ sin φ + Bpz sin θ cos φ ⎟⎟
⎝
⎠
Таким образом, найдены выражения для определения всех трех углов
поворота датчика.
74
МАГНИТНО-ИНЕРЦИАЛЬНЫЙ МЕТОД
ОПРЕДЕЛЕНИЯ ПОЛОЖЕНИЯ
И ОРИЕНТАЦИИ ОБЪЕКТА
ВЕСТНИК ТОГУ. 2014. № 1(32)
Определение координат
В общем случае, процесс вычисления координат можно описать следуuur
ющим образом. Полученный вектор индукции магнитного поля катушки Bs
необходимо перевести из координат датчика в координаты устройства, с целью компенсировать поворот датчика. Для этой цели его надо повернуть на
углы, обратные углам [φ ,θ ,ψ ] , в обратном порядке поворота:
⎡B ⎤
uur ⎢ cx ⎥
uur
Bc = ⎢ Bcy ⎥ = Rz (−ψ ) Ry (−θ ) Rx (−φ ) Bs ,
⎢⎣ Bcz ⎥⎦
uur
где Bc - вектор измеренной индукции магнитного поля катушки после коррекции на углы крена, тангажа и рыскания датчика, т.е. при φ = θ = ψ = 0 .
Далее остается перейти от вектора к его координатам. В данной работе
предлагается использовать функцию координат от вектора, заданную таблично. Элементами таблицы являются пары: вектор магнитной индукции и координаты этого вектора. Вычисление вектора магнитной индукции, соответствующего определенной точке, производится с помощью закона БиоСавара-Лапласа.
Если использовать катушку индуктивности цилиндрической формы
(соленоид), то ее осевая симметрия позволит перейти в сферические координаты (рис. 3).
Рис. 3. Сферическая система координат, связанная с катушкой
Любой вектор магнитной индукции, создаваемый катушкой, лежит в
плоскости, проходящей через ось симметрии катушки z. Следовательно,
угол ϕ c между проекцией этого вектора на плоскость x0y и осью x совпадает с угловой координатой ϕ данной точки пространства, и выражение
для угла ϕ имеет следующий вид:
75
Чье Ен Ун, Чернышов Е. С.
ВЕСТНИК ТОГУ. 2014 № 1 (32)
tan ϕ = tan ϕc =
Bcy
.
Bcx
С помощью таблицы остается произвести поиск оставшихся координат r и θ точки. Таблица при этом может быть составлена только для одного произвольного квадранта (четверти плоскости), ограниченного вертикальной осью симметрии z и горизонтальной плоскостью катушки x0y.
Для простоты расчетов удобно использовать первый квадрант плоскости
x0z (на рис. 3 – заштрихованная область Q).
На рис. 4 приведен пример данных, хранимых в таблице. Данные
предлагается хранить также в сферических координатах. Так как координата ϕ = ϕ c вектора и точки известна, то оставшимися данными, характеризующими вектор магнитной индукции, являются его длина rc и угол θ c
между вектором и осью z. А оставшимися координатами, характеризующими точку в пространстве, являются расстояние до начала координат r и
угол θ между отрезком, соединяющим точку с началом координат, и осью z.
Рис. 4. Пример данных, хранимых в таблице
uur
Вектор Bc преобразуется из декартовых координат в сферические с
помощью следующих формул:
rc = Bcx 2 + Bcy 2 + Bcz 2 ,
cosθ c =
76
Bcz
.
rc
МАГНИТНО-ИНЕРЦИАЛЬНЫЙ МЕТОД
ОПРЕДЕЛЕНИЯ ПОЛОЖЕНИЯ
И ОРИЕНТАЦИИ ОБЪЕКТА
ВЕСТНИК ТОГУ. 2014. № 1(32)
Далее производится поиск в таблице наиболее близкого вектора (по
длине rc и углу θ c ), его координаты и являются искомыми координатами
точки.
Следует отметить, что кроме значительного уменьшения объема данных в таблице, представление данных в сферической системе координат
позволяет, при необходимости, осуществлять интерполяцию данных в
таблице значительно более простыми средствами (например, с помощью
билинейной интерполяции).
При необходимости, перевод координат точки в декартову систему
осуществляется с помощью известных выражений:
x = r sin θ cos ϕ ,
y = r sin θ sin ϕ ,
z = r cosθ .
Необходимо также отметить одно важное ограничение описанного в
статье метода. Распределение магнитного поля в полупространствах по
обе стороны катушки зеркально симметрично с противоположным знаком
относительно плоскости катушки. Таким образом, имея информацию
только о величине и направлении вектора магнитной индукции, невозможно определить, какому полупространству принадлежит точка. Следовательно, для устранения неопределенности, необходимо ограничивать
область перемещения одним полупространством, использовать дополнительные технические средства для определения полупространства, или
анализировать динамику перемещений объекта для оценки текущего полупространства.
Библиографические ссылки
1. Raab F., Bood E., Steiner O., Jones H. Magnetic position and orientation tracking
system // IEEE Transactions on Aerospace and Electronics Systems. - AES-15(5), Sept.
1979. - P. 709-717.
2. Смирнов Б.М. Устройство для определения положения объекта (варианты):
пат. 2171476 Рос. Федерация : МПК G01R33/02, G01C21/04 – № 2000114436/09 ; заявл. 05.06.2000 ; опубл. 27.07.2001.
3. Трофимова Т.И. Курс физики: учеб. пособие для вузов – 11-е изд., стер. – М.:
Издательский центр «Академия», 2006. – 560 с.
4. Чернышов Е.С. Метод локального позиционирования с использованием магнитного поля // Материалы секционных заседаний 52-й студенческой научнотехнической конференции ТОГУ. – Хабаровск : Изд-во Тихоокеан. гос. ун-та, 2012. С. 133-137.
5. Herbert R. Jones. Method and Apparatus for Determining Remote Object Orientation and Position: US Patent 4737794, Apr. 12, 1988.
6. Talat Ozyagcilar. Implementing a Tilt-Compensated eCompass using Accelerome-
77
ВЕСТНИК ТОГУ. 2014 № 1 (32)
Чье Ен Ун, Чернышов Е. С.
ter and Magnetometer Sensors // Freescale Semiconductor Application Note. - Document
Number AN4248, Revision 3, January 2012.
Title: Magnetic-Inertial Technique for Determining Object Position and Orientation
Authors’ affiliation:
Chye En Un – Pacific National University, Khabarovsk, Russian Federation
Chernyshov E. S. – Pacific National University, Khabarovsk, Russian Federation
Abstract: The hybrid magnetic-inertial technique for determining object position and orientation, using one inductor coil and the modern digital three-axis MEMS sensors - magnetometer and accelerometer, is described. Algorithms for computation of the sensor rotation
angles and coordinates are given. The method for conversion of the magnetic induction
vector to the sensor coordinates using the table function is proposed.
Keywords: position and orientation tracking, magnetic field, magnetometer, accelerometer,
Biot–Savart–Laplace’s law, rotation matrix, spherical coordinates.
78
1/--страниц
Пожаловаться на содержимое документа