[Из песочницы] МЭМС акселерометры, магнитометры и углы ориентации07.03.2020 18:23

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

Чтобы понять, на какие точности углов мы можем рассчитывать, нужно приложить некоторое количество усилий.

TL; DR: Описан небольшой скрипт для Octave/MATLAB, позволяющий оценить ошибки расчёта углов ориентации по измерениям МЭМС акселерометров и магнитометров. На входе скрипта — параметры датчиков из даташитов (и/или погрешности калибровки). Статья может быть полезна тем, кто начинает использовать инерциальные датчики в своих устройствах. Небольшой ликбез по датчикам прилагается. Ссылка на гитхаб тоже.
Сходу примем такие условия:

• Мы хотим оценить углы ориентации неподвижного устройства.
  Почему?

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

• Для оценки углов мы будем использовать измерения МЭМС акселерометров и магнитометров.

1. Краткий ликбез

Углы ориентации

Будем понимать под углами ориентации объекта углы Эйлера — крен (roll), тангаж (pitch), рыскание (yaw), связывающие собственную систему координат XYZ объекта и локальную систему координат восток-север-верх (ENU — East North Up). Углы roll, pitch, yaw обозначают поворот, который нужно совершить осям XYZ чтобы перейти в оси ENU. Соответственно, нулевые углы означают, что ось X объекта смотрит на восток, ось Y объекта смотрит на север, ось Z — вверх.

Порядок поворота осей — начиная с последнего угла: сначала на yaw (вокруг оси Z), потом на pitch (вокруг оси Y), потом на roll (вокруг оси X).

Акселерометр

Это датчик, измеряющий проекцию кажущегося ускорения на ось чувствительности. Кажущегося — потому что измеряет и силу тяжести тоже, даже в то время как акселерометр неподвижен. Проще всего представить акселерометр как грузик на пружинке, его выдаваемые измерения пропорциональны степени растяжения пружины. Если акселерометр покоится — пружина растянута лишь силой тяжести. Если ускоряется — то будет сумма сил: инерции грузика $inline$\left (F=m\overrightarrow{a}\right)$inline$ и силы тяжести $inline$\left (F_{g}=m\overrightarrow{g}\right)$inline$

Примем следующую модель измерений триады ортогональных (взаимно перпендикулярных) акселерометров:

$$display$$a_{XYZ}= m_{a} \cdot A_{XYZ}+b_{a}+n_{a},$$display$$

где $inline$a_{XYZ}$inline$ — измеряемое ускорение в ССК (собственной системе координат) XYZ, $inline$m_{a}$inline$ — матрица перекоса осей и масштабных коэффициентов акселерометра, $inline$A_{XYZ}$inline$ — вектор истинного ускорения в ССК XYZ, $inline$b_{a}$inline$ — вектор смещения нуля акселерометра, $inline$n_{a}$inline$ — шум измерений.

Матрица перекоса осей и масштабных коэффициентов выглядит следующим образом:

$$display$$m_{a}=\begin{bmatrix}1+m_{a,1,1} & m_{a,1,2} & m_{a,1,3}\\ m_{a,2,1}& 1+m_{a,2,2} & m_{a,2,3}\\ m_{a,3,1} & m_{a,3,2} & 1+m_{a,3,3} \end{bmatrix},$$display$$

где элементы, расположенные по главной диагонали ($inline$ 1+m_{a,1,1}, 1+m_{a,2,2}, 1+m_{a,3,3}$inline$) — это масштабные коэффициенты и их погрешности по трём осям акселерометра, а остальные элементы матрицы — перекосы осей акселерометра.

Магнитометр

Датчик, который измеряет проекцию индукции магнитного поля на ось чувствительности. Магнитометру свойственны искажения hard-iron и soft-iron. Hard-iron искажение — это аддитивный эффект, когда к измеряемому полю добавляется постоянная составляющая. Причиной может быть, например, действие постоянного магнита или собственное смещение нуля датчика. Искажение soft-iron — мультипликативный эффект, отражающий изменение направления и/или ослабление вектора магнитной индукции. Этот эффект может быть вызван наличием металлического предмета в непосредственной близости от магнитометра или же собственными искажениями датчика — погрешностью масштабного коэффициента или перекосом его оси чувствительности.
Примем модель измерений триады магнитометров:

$$display$$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m},$$display$$

где $inline$m_{XYZ}$inline$ — измерения магнитометра в ССК XYZ, $inline$S_{m}$inline$ — диагональная матрица перекоса осей и масштабных коэффициентов (которая описывает эффект soft–iron), $inline$M_{XYZ}$inline$ — вектор истинной магнитной индукции в ССК, $inline$b_{m}$inline$ — смещение нулей магнитометра (описывает эффект hard–iron), $inline$n_{m}$inline$ — шум измерений.
Матрица перекоса осей и масштабных коэффициентов магнитометра:

$$display$$S_{m}=\begin{bmatrix}1+S_{m,1,1} & S_{m,1,2} &S_{m,1,3}\\ S_{m,2,1}& 1+S_{m,2,2} & S_{m,2,3}\\ S_{m,3,1} & S_{m,3,2}&1+ S_{m,3,3} \end{bmatrix},$$display$$

элементы, расположенные на главной диагонали ($inline$S_{m,1,1}, S_{m,2,2}, S_{m,3,3}$inline$) — масштабные коэффициенты и их погрешности по трём осям магнитометра, остальные элементы матрицы — перекосы осей магнитометра. Все элементы матрицы также учитывают эффект soft-iron.

Расчет углов ориентации

Благодаря наличию на Земле силы тяжести, акселерометры «чувствуют» направление вниз. Их измерения используются для расчета углов крена и тангажа. Формулы для расчёта можно найти тут. Третий — угол рыскания (а в данном случае — магнитного азимута), может быть определен благодаря наличию у Земли магнитного поля. Вектор индукции магнитного поля измеряется магнитометрами и их измерения участвуют в расчете угла рыскания. Нужно отметить, что в расчёте магнитного азимута используются измерения магнитометра, пересчитанные в плоскость. Здесь можно найти формулу для расчёта магнитного азимута.

$$display$$roll=atan\left (\frac{a_{Y}}{a_{Z}} \right),$$display$$

$$display$$pitch=atan\left (\frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right),$$display$$

$$display$$yaw=atan2\left (\frac{m_{E}}{m_{N}} \right),$$display$$

где $inline$atan2$inline$ — функция полного арктангенса, $inline$a_{X}$inline$, $inline$a_{Y}$inline$, $inline$a_{Z}$inline$ — измерения акселерометра по трём осям в ССК, $inline$m_{E}$inline$, $inline$m_{N}$inline$ — измерения магнитометра по осям X', Y' (измерения магнитометров пересчитаны в плоскость).

2. Ошибки оценивания углов ориентации

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

• Сформируем массивы случайных углов Эйлера roll, pitch, yaw. Они будут задавать наборы вариантов истинной ориентации объекта в модели.
  Зачем много углов?

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

• Из случайных углов roll, pitch, yaw формируется матрица преобразования из ССК XYZ в ЛСК ENU:

  $$display$$C_{XYZ}^{ENU}=\begin{vmatrix} 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{vmatrix},$$display$$

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

  $$display$$A_{XYZ}=\left (C_{XYZ}^{ENU} \right)^{T}\cdot \begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix},$$display$$

  
  $inline$\begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix}$inline$ — вектор, определяющий направление гравитационного ускорения, выраженный в единицах g, $inline${(C_{XYZ}^{ENU})}^{T}$inline$ — матрица преобразования координат из ЛСК в ССК (обратная матрице преобразования из ССК в ЛСК).
• Применяем модель измерения акселерометра:

  $$display$$a_{XYZ}=\left (I+m_{a} \right)\cdot A_{XYZ}+b_{a}+n_{a},$$display$$

• По измерениям акселерометра рассчитываются новые углы крена и тангажа (оценки) по формулам:

  $$display$$roll'=atan \left (\frac{a_{Y}}{a_{Z}} \right),$$display$$

  $$display$$pitch'=atan\left (\frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right).$$display$$

• Также необходимо сформировать матрицу пересчета в «горизонт» из этих углов, для этого воспользуемся функцией rpy2mat:

  $$display$$C_{XYZ}^{XYZ'}=rpy2mat\left (\begin{bmatrix} roll'\\ pitch'\\ 0 \end{bmatrix}^{T} \right),$$display$$

  где углы roll' и pitch' — это углы, рассчитанные по измерениям акселерометра, а третий угол — нулевой.
• Возьмем вектор истинных магнитных плотностей в ЛСК ENU и пересчитаем его в ССК XYZ:

  $$display$$M_{XYZ}= {(C_{XYZ}^{ENU})}^{T}\cdot M_{ENU}.$$display$$

• Применяем модель измерений магнитометра:

  $$display$$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m}.$$display$$

• Осталось пересчитать измерения магнитометра из ССК в «горизонт»:

  $$display$$m_{XYZ'}=C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$$display$$

• По «горизонтированным» измерениям магнитометра расcчитывается угол магнитного азимута:

  $$display$$yaw'=atan2\left (\frac{m_{Y'}}{m_{X'}} \right).$$display$$

• Ошибки оценивания углов ориентации рассчитываются как разность между истинными углами roll, pitch, yaw и рассчитанными по измерениям датчиков — roll', pitch', yaw'.

3. Результаты — расчет погрешностей оценки углов

Для двух датчиков, которые мы взяли для примера — ADIS16488A и MPU-9250, получены предельные ошибки оценивания углов ориентации при совместном влиянии погрешностей акселерометра и магнитометра.

В таблице ниже — максимальные значения полученных ошибок:

Совместное влияние погрешностей акселерометра и магнитометра на ошибки оценивания углов ориентации:

• Вот так выглядят ошибки оценивания крена в зависимости от значений крена и тангажа:

  

• Ошибки оценивания тангажа в зависимости от значений крена и тангажа:

  

• Ошибки оценивания магнитного азимута от углов крена и тангажа:

  

• Ошибки оценивания магнитного азимута от углов крена и магнитного азимута:

  

• Ошибки оценивания магнитного азимута от углов тангажа и магнитного азимута:

  

Итог

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

Литература

© Habrahabr.ru