05.04.2024 19:37
da-nie 13 2300 Источник


В настоящее время широко доступны датчики на основе магнитометров. Они позволяют легко получить направление на магнитный север (или истинный, если, конечно, вы учтёте магнитное склонение в вашей местности). Это может быть полезно для определения ориентации оси рысканья/курса беспилотных аппаратов. Одна из проблем магнитометров связана с их калибровкой, поскольку на магнитометр влияют находящиеся рядом с ним магнитотвёрдые (имеют собственное магнитное поле) и магнитомягкие (легко намагничиваются от внешнего магнитного поля, в том числе от поля Земли) материалы. Ниже я расскажу, как избавиться от влияния этих материалов и откалибровать магнитометр.

Про калибровку магнитометров писали уже неоднократно. Однако, калибровка обобщённым фильтром Калмана учёта магнитомягких материалов обнаружилась лишь в статьях Extended Kalman Filter Based Gyroscope (авторы Ke Han, He Han, Zhifeng Wang, Feng Xu) и Real-Time Magnetic Field Calibration Method Based on Extended Kalman Filter (авторы LI Wenkuan, CAI Haoyuan, ZHAO Shenglin, LIU Chunxiu). Возможно, есть и другие статьи, но я ориентировался на вот эти. Так как сходу разобраться в этих статьях не так-то просто (я разбирался две недели с коллегой-математиком, ибо в статьях есть и ошибки и нет акцента на важных моментах из-за чего легко ошибиться при построении собственного фильтра), то ниже я дам пошаговый алгоритм калибровки магнитометра по методике из этих статей.

Итак, поехали!

1 Основные положения


Пусть имеется внешнее магнитное поле $\overrightarrow{B_{\text{вн}}}$ (например, магнитное поле Земли :) ). Тогда истинное значение вектора магнитного поля при вращении определяется как $\overrightarrow{B_{\text{ист}}}=\mathbf{R}\cdot\overrightarrow{B_{\text{вн}}}$, где $\mathbf{R}$ — матрица поворота. Влияние магнитомягких и магнитотвёрдых материалов (всё это вращается вместе с магнитометром, естественно! Например, это могут быть корпуса микросхем или элементы конструкции.) на показания магнитометров можно описать с помощью матрицы анизотропии магнитной проницаемости $\mathbf{W}$ и вектора смещения нуля $\overrightarrow{V}$ магнитометра, соответственно. В этом случае показания магнитометра (без учёта шума измерения) будут определяться как $\overrightarrow{B_{\text{изм}}}=\mathbf{W}\cdot\mathbf{R}\cdot\overrightarrow{B_{\text{вн}}}+\overrightarrow{V}$
, где $\overrightarrow{V}=\left\{ V_{X},V_{Y},V_{Z}\right\}$ и $\mathbf{W}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{21} & W_{22} & W_{23}\\ W_{31} & W_{32} & W_{33} \end{array}\right]$. Матрица $\mathbf{W}$ должна быть симметричная, что позволяет задать её как $\mathbf{W}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{12} & W_{22} & W_{23}\\ W_{13} & W_{23} & W_{33} \end{array}\right]$.
Для определения матрицы $\mathbf{W}$ и вектора $\overrightarrow{V}$ можно применить обобщённый фильтр Калмана.

Пусть вектор состояния $\overrightarrow{X}=\left\{ \overrightarrow{B_{\text{ист}}},W_{11},W_{22},W_{33},W_{12},W_{13},W_{23},\overrightarrow{V}\right\}$
$=\left\{ B_{\text{ист}X},B_{\text{ист}Y},B_{\text{ист}Z},W_{11},W_{22},W_{33},W_{12},W_{13},W_{23},V_{X},V_{Y},V_{Z}\right\}$
содержит предсказанные фильтром истинные значения магнитного поля $\overrightarrow{B_{\text{ист}}}=\left\{ B_{\text{ист}X},\:B_{\text{ист}Y},\:B_{\text{ист}Z}\right\}$, элементы матрицы $\mathbf{W}$ (эта матрица симметричная, поэтому достаточно всего 6 элементов) и определённые смещения нулей магнитометров $V_{X},V_{Y},V_{Z}$.

На каждой итерации работы фильтра должны быть известны показания измерений магнитометра $\overrightarrow{B_{\text{изм}}}$ и три угла вращения (крен $roll$, тангаж $pitch$ и рысканье $yaw$) по которым задаётся матрица поворота $\mathbf{R}$, имеющая для каждой итерации $k$ вид

$\mathbf{R_{k}}=\left[\begin{array}{ccc} cy\cdot cp & -cr\cdot sy+sr\cdot cy\cdot sp & sr\cdot sy+cr\cdot cy\cdot sp\\ sy\cdot cp & cr\cdot cy+sr\cdot sy\cdot sp & -sr\cdot cy+cr\cdot sy\cdot sp\\ -sp & sr\cdot cp & cr\cdot cp \end{array}\right],$


где $cr=cos(roll),\:sr=sin(roll)$, $\:cp=cos(pitch),\:sp=sin(pitch)$, $\:cy=cos(yaw),\:sy=sin(yaw)$.
Сами абсолютные значения углов вращения как таковые значения не имеют, но вот их изменения от итерации к итерации должны соответствовать действительному изменению углов. То есть, если был поворот на 10 градусов между итерациями, то переданный на следующую итерацию угол должен быть изменён на эти же 10 градусов. Так-то эта матрица вовсе не нужна, если вы можете построить матрицу вращения за итерацию непосредственно по гироскопам.

2 Инициализация фильтра Калмана


1. Инициализируем вектор состояний шага $k=0$ как $X_{0}=\left\{ B_{\text{изм}X},B_{\text{изм}Y},B_{\text{изм}Z},1,1,1,0,0,0,0,0,0\right\}$. То есть вектор $\overrightarrow{B_{\text{ист}}}$ приравняем к текущим показаниям магнитометров, матрицу $\mathbf{W}$ зададим единичной, а вектор $\overrightarrow{V}$ зададим нулевым.

2. Инициализируем ковариационную матрицу состояний $\mathbf{P\mathrm{_{0}=\left[I_{9x9}\right]}}$. На диагонали этой матрицы расположены дисперсии изменений элементов вектора состояний. Имеет смысл поставить для инициализации максимальное значение, перекрывающее диапазон изменения элемента вектора состояний. У меня в модели показания магнитометров изменяются от -1 до 1, а потому я задаю эту матрицу единичной.

3. Инициализируем матрицу вращения предыдущего шага $\mathbf{R}_{0}$ равной текущей матрице вращения, т.е. $\mathbf{R}_{0}=\mathbf{R}_{k}$.

3 Предсказание


1. Вычисляем изменение матрицы поворота $\triangle\mathbf{R}=\mathbf{R}_{k}^{T}\cdot\mathbf{R}_{k-1}$.

2. Задаём матрицу динамики (прогнозирования) $\mathbf{F}$. Так как у нас обобщённый фильтр Калмана, матрица динамики должна содержать частные производные функции динамики $\overrightarrow{f}(\overrightarrow{X})$. Функция динамики определяется как

$\overrightarrow{f}(\overrightarrow{X_{k-1}})=\left[\begin{array}{c} B_{\text{ист}X,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{11}+B_{\text{ист}Y,k-1}\cdot\triangle R_{12}+B_{\text{ист}Z,k-1}\cdot\triangle R_{13}\\ B_{\text{ист}Y,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{21}+B_{\text{ист}Y,k-1}\cdot\triangle R_{22}+B_{\text{ист}Z,k-1}\cdot\triangle R_{23}\\ B_{\text{ист}Z,k}=B_{\text{ист}X,k-1}\cdot\triangle R_{31}+B_{\text{ист}Y,k-1}\cdot\triangle R_{32}+B_{\text{ист}Z,k-1}\cdot\triangle R_{33}\\ W_{11,k}=W_{11,k-1}\\ W_{22,k}=W_{22,k-1}\\ W_{33,k}=W_{33,k-1}\\ W_{12,k}=W_{12,k-1}\\ W_{13,k}=W_{13,k-1}\\ W_{23,k}=W_{23,k-1}\\ V_{X,k}=V_{X,k-1}\\ V_{Y,k}=V_{Y,k-1}\\ V_{Z,k}=V_{Z,k-1} \end{array}\right]$


Таким образом, матрица частных производных функции динамики (якобиан) определяется как

$\mathbf{F}=\frac{\partial\overrightarrow{f}(\overrightarrow{X})}{d\overrightarrow{X}}=\left[\begin{array}{cccccccccccc} \triangle R_{11} & \triangle R_{12} & \triangle R_{13} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ \triangle R_{21} & \triangle R_{22} & \triangle R_{23} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ \triangle R_{31} & \triangle R_{32} & \triangle R_{33} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right]$


$=\left[\begin{array}{cc} \mathbf{\bigtriangleup R}_{3x3} & 0_{3x9}\\ 0_{9x3} & I_{9x9} \end{array}\right]$



3. Вычисляем прогноз вектора состояний $\mathbf{\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}}$. Так как функция $\overrightarrow{f}(\overrightarrow{X})$ линейная, то $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}=\overrightarrow{f}(\overrightarrow{X})=\mathbf{F}\cdot\overrightarrow{X_{k-1}}$.

4. Вычисляем прогноз ковариационной матрицы состояний $\mathbf{P}=\mathbf{F}\cdot\mathbf{P}_{k-1}\cdot\mathbf{F^{T}}+\mathbf{Q}$. Для этого прогноза должна быть известна матрица ковариации ошибки модели $\mathbf{Q}$. Если в этой матрице установить очень маленькие значения, то это будет значить, что мы считаем, что наша модель точно описывает процесс. Если же установить большые значения в $\mathbf{Q}$, то это означает, что наша модель может содержать неточности. В нашем случае будем считать, что модель точно описывает процесс, а потому матрицу $\mathbf{Q}$ сделаем равной нулю. В работе Real-Time Magnetic Field Calibration Method Based on Extended Kalman Filter предложена модель для каждой итерации фильтра использовать $\mathbf{Q\mathrm{_{k-1}=\left[\begin{array}{cc} \mathbf{A}_{3x3} & 0_{3x9}\\ 0_{9x3} & 0_{9x9} \end{array}\right]}}$
, где $\mathbf{A}=\overrightarrow{B}_{\text{изм}}\cdot\overrightarrow{B}_{\text{изм}}^{T}\cdot\mathbf{\sigma_{\triangle R}^{2}}\cdot\bigtriangleup t^{2}$. Значение $\bigtriangleup t$ — интервал работы фильтра, а матрица
$\mathbf{\sigma_{\triangle R}^{2}}$ определена из эксперимента для используемого оборудования как $\mathbf{\sigma_{\triangle R}^{2}}=\left[\begin{array}{ccc} 1.2\cdot10^{\text{−}8} & 3.0\cdot10^{\text{−}3} & 1.4\cdot10^{\text{−}3}\\ 3.0\cdot10^{\text{−}3} & 4.0\cdot10^{\text{−}8} & 2.5\cdot10^{\text{−}3}\\ 1.4\cdot10^{\text{−}3} & 2.5\cdot10^{\text{−}3} & 6.5\cdot10^{\text{−}8} \end{array}\right]$. Если я напрямую применю их матрицу (а почему бы и нет для моделирования-то?), мой фильтр разваливается. Это потому, что диапазон показаний датчиков у них вовсе не -1...1, а гораздо больше. Поэтому для моего диапазона все значения надо поделить эдак на 10000.

4 Корректировка


1. Из полученного прогнозного вектора состояний $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}$ можно построить матрицу измерений $\mathbf{H}$. Для обобщённого фильтра Калмана она должна содержать частные производные функции измерений

$\overrightarrow{h}(\overrightarrow{X)}=\left[\begin{array}{c} B_{\text{изм}X}=B_{\text{ист}X}\cdot W_{11}+B_{\text{ист}Y}\cdot W_{12}+B_{\text{ист}Z}\cdot W_{13}+V_{X}\\ B_{\text{изм}Y}=B_{\text{ист}X}\cdot W_{21}+B_{\text{ист}Y}\cdot W_{22}+B_{\text{ист}Z}\cdot W_{23}+V_{Y}\\ B_{\text{изм}Z}=B_{\text{ист}X}\cdot W_{31}+B_{\text{ист}Y}\cdot W_{32}+B_{\text{ист}Z}\cdot W_{33}+V_{Z} \end{array}\right]$

.
Таким образом, с учётом симметричности матрицы $\mathbf{W}$, мы получаем якобиан

$\mathbf{H}=\frac{\partial\overrightarrow{h}(\overrightarrow{X})}{d\overrightarrow{X}}=\left[\begin{array}{ccc} W_{11} & W_{12} & W_{13}\\ W_{12} & W_{22} & W_{23}\\ W_{13} & W_{23} & W_{33} \end{array}\begin{array}{ccc} B_{\text{изм}X} & 0 & 0\\ 0 & B_{\text{изм}Y} & 0\\ 0 & 0 & B_{\text{изм}Z} \end{array}\begin{array}{ccc} B_{\text{изм}Y} & B_{\text{изм}Z} & 0\\ B_{\text{изм}X} & 0 & B_{\text{изм}Z}\\ 0 & B_{\text{изм}X} & B_{\text{изм}\text{Y}} \end{array}\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{array}\right]$

.
Элементы этой матрицы нужно взять из прогнозного вектора состояний $\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}}}$.

2. Применим функцию измерений к предсказанному вектору состояний:
$\overrightarrow{D}=f(\mathbf{\overrightarrow{\mathrm{X_{\text{прогноз}}}})=W\cdot\overrightarrow{\mathrm{B_{\text{ист}}}}+\mathrm{\overrightarrow{V}}}$.

3. Вычисляем ошибку $\overrightarrow{Y}=\left[\begin{array}{c} B_{\text{изм}X}\\ B_{\text{изм}Y}\\ B_{\text{изм}Z} \end{array}\right]-\overrightarrow{D}$.

4. Зададим матрицу ковариации шума измерений $\mathbf{E}=\sigma^{2}\cdot\mathbf{I}_{3x3}$. Для шума $\sigma^{2}$ примем равной $0.00001$ (для моего диапазона показаний от -1 до 1 я взял такое число для моделирования). Считаем, что измерения не влияют друг на друга, а потому присутствуют только диагональные элементы матрицы.

5. Считаем усиление фильтра: $\mathbf{K}=\mathbf{P}\cdot\mathbf{H}^{T}\cdot\mathbf{S}^{-1}$, где $\mathbf{S}=\mathbf{H}\cdot\mathbf{P}\cdot\mathbf{H}^{T}+\mathbf{E}$.

6. Обновляем вектор состояния: $\overrightarrow{X_{k}}=\overrightarrow{\mathrm{X_{\text{прогноз}}}}+\mathbf{K}\cdot\overrightarrow{Y}$.

7. Обновляем ковариационную матрицу состояний: $\mathbf{P}_{k}=(\mathbf{I}_{12x12}-\mathbf{K}\cdot\mathbf{H})\cdot\mathbf{P}$.

8. Обновляем старые переменные в новое состояние: $\overrightarrow{X_{k-1}}=\overrightarrow{X}_{k} , \mathbf{P}_{k-1}=\mathbf{P}_{k}, \mathbf{R}_{k-1}=\mathbf{R}_{k}$.

9. Переходим к первому пункту этапа Предсказание.

Через некоторое время в компонентах вектора $\overrightarrow{X_{k}}$ будет определена матрица учёта магнитомягких материалов и смещений. Осталось только из реальных показаний магнитометра вычесть эти смещения и умножить полученный вектор на обратную матрицу учёта магнитомягких материалов $\mathbf{W}$. То, что вышло и есть искомый вектор внешнего магнитного поля. Вот и всё! Хочу, правда, предупредить, что при больших значениях (десятые доли) матрицы $\mathbf{W}$ её компоненты могут быть определены не очень точно. То же касается и смещений. Во всяком случае, у меня при моделировании получалось так.

Успешных калибровок! :)

Экспериментальная программа. Буквально на попробовать. Я такое даже на свой гитхаб класть не хочу. Но все операции с матрицами там есть.

Комментарии (13)


  1. IgorRJ
    05.04.2024 20:16
    #26693631
    +5

    Направление на магнитный север не требует учета магнитного склонения. Оно нужно для определения направления на истинный север при известном направлении на магнитный.


    1. da-nie Автор
      05.04.2024 20:16
      #26694139

      Ступил. :) Исправлю.


  1. diakin
    05.04.2024 20:16
    #26693993

    А зачем трехмерный вектор рассматривать, одномерного разве не достаточно?
    И нам же не требуется величину вектора определять, только направление. Поэтому разве будет влиять анизотропия на результат - направление вектора?


    1. da-nie Автор
      05.04.2024 20:16
      #26694153
      +2

      Наверное, вы имели в виду двумерный вектор, а не одномерный? Магнитометр в приборе вовсе не всегда будет находиться в плоскости горизонта, поэтому используются все три координаты.

      Анизотропия влиять на направление будет. Если бы она не влияла, матрица W осталась бы единичной.


      1. diakin
        05.04.2024 20:16
        #26694259

        Да, двухмерный конечно, в плоскости )
        Ну да, когда магнитометр наклоняется по одной из осей X\Y, одна компонента уменьшается, а вторая нет, соответственно суммарный вектор будет менять направление.
        А анизотропия как влияет? Магнитомягкий материал усиливает внешнее магнитное поле, когда оно совпадает с направлением анизотропии, как-то так? Но направление вектора магнитного поля от этого не меняется же?


        1. Radisto
          05.04.2024 20:16
          #26694355
          +1

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


        1. da-nie Автор
          05.04.2024 20:16
          #26694359

          Магнитомягкие материалы постоянно добавляют свой вектор магнитного поля, совпадающий с полем Земли+поле магнитотвёрдого материала около них+поле другого магнитомягкого материала около них, причём, эти магнитомягкие материалы находятся где-то сбоку от магнитометра (но вращаются вместе с ним), и поэтому результирующий вектор, действующий на магнитометр получается как сумма всех этих векторов. Вот если бы магнитомягкие материалы находились бы внутри магнитометра, тогда никакого искажения не было бы.


  1. black_list_man
    05.04.2024 20:16
    #26694439

    Исходников в открытом доступе нет случайно?


    1. da-nie Автор
      05.04.2024 20:16
      #26694451
      +1

      Есть. Только это будет просто экспериментальная программа. Буквально на попробовать. Я такое даже на свой гитхаб класть не хочу. Но все операции с матрицами там есть.

      Вот она.


  1. AndrewSu
    05.04.2024 20:16
    #26695585

    А зачем фильтр Калмана? Можно же просто составить систему уравнений и решить её...


    1. da-nie Автор
      05.04.2024 20:16
      #26697495

      А она может и не решиться. Шумы измерений не позволят.


  1. aax
    05.04.2024 20:16
    #26698789

    Вопрос несколько вскользь. В последнее время в платах объеденяющие акселерометр, гироскоп и магнитометр(пришедших на смену снятым с производства 9250 с HMC5883L) не могу нормально опросить магнитометр. Максимум что получается это считать мусорные константы от магнитного поля не зависящие.

    Отдельные без акселерометра платы с магнитометром QMC5883L, например, работают без проблем, а магнитометров на новых платах 9000 серии вроде этих https://roboshop.spb.ru/sensors/datchiki-navigacii/mpu9250-module никаким из известных мне способов получить данные коррелирующие с магнитным полем мне не удаётся. Обычно магнитометром там указывают AK8963. Судя по комментариям в различных магазинах эта проблема не только у меня.

    Это я что-то не знаю про особенности опроса магнитометров на новых 9000 платах, или действительно массовый брак пошёл?


    1. da-nie Автор
      05.04.2024 20:16
      #26700925

      вроде этих https://roboshop.spb.ru/sensors/datchiki-navigacii/mpu9250-module

      Так. Погодите-ка. Я только что такую плату на 9250 вчера получил. И вчера попытался впервые её опросить по SPI. Может, я неправильно опрашиваю магнитометр, но только данных я нифига не получил (принимаются застывшие биты). Я вообще думал, сама 9250 внутри с магнитометром. И он там этот самый аналог AK8963. И я как-то не догадывался, что в 9250 внутри меняют схемотехнику. Отвратительно.