- PVSM.RU - https://www.pvsm.ru -
При разработке устройств, в которых необходимо оценивать углы ориентации, может встать вопрос — какой МЭМС датчик выбрать. Производители датчиков приводят множество параметров, из которых нам требуется получить полезную информацию о качестве датчика.
Чтобы понять, на какие точности углов мы можем рассчитывать, нужно приложить некоторое количество усилий.
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=moverrightarrow{a}right )$inline$ и силы тяжести $inline$left (F_{g}=moverrightarrow{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$) — это масштабные коэффициенты и их погрешности по трём осям акселерометра, а остальные элементы матрицы — перекосы осей акселерометра.
Зная спектральную плотность мощности шума и полосу пропускания датчика можно рассчитать СКО шума на выходе датчика:
$$display$$sigma _{noise}=G_{0}cdotsqrt{Pi_{noise}};$$display$$
Датчик, который измеряет проекцию индукции магнитного поля на ось чувствительности. Магнитометру свойственны искажения 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.
В даташите нужных нам параметров нет, поэтому предположим, что магнитометр откалиброван и возьмем следующие числа:
Благодаря наличию на Земле силы тяжести, акселерометры «чувствуют» направление вниз. Их измерения используются для расчета углов крена и тангажа. Формулы для расчёта можно найти тут [2]. Третий — угол рыскания (а в данном случае — магнитного азимута), может быть определен благодаря наличию у Земли магнитного поля. Вектор индукции магнитного поля измеряется магнитометрами и их измерения участвуют в расчете угла рыскания. Нужно отметить, что в расчёте магнитного азимута используются измерения магнитометра, пересчитанные в плоскость. Здесь [3] можно найти формулу для расчёта магнитного азимута.
$$display$$roll=atanleft ( frac{a_{Y}}{a_{Z}} right ),$$display$$
$$display$$pitch=atanleft ( frac{-a_{X}}{sqrt{a_{Y}^{2}+a_{Z}^{2}}} right ),$$display$$
$$display$$yaw=atan2left ( 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' (измерения магнитометров пересчитаны в плоскость).
$$display$$C_{XYZ}^{ENU}=begin{vmatrix} cycdot cp &-crcdot sy+srcdot cycdot sp &srcdot sy+crcdot cycdot sp \ sycdot cp &crcdot cy+srcdot sycdot sp & -srcdot cy+crcdot sycdot sp \ -sp &srcdot cp &crcdot 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'=atanleft ( frac{-a_{X}}{sqrt{a_{Y}^{2}+a_{Z}^{2}}} right ).$$display$$
$$display$$C_{XYZ}^{XYZ'}=rpy2matleft ( begin{bmatrix} roll'\ pitch'\ 0 end{bmatrix}^{T} right ),$$display$$
где углы roll' и pitch' — это углы, рассчитанные по измерениям акселерометра, а третий угол — нулевой.
$$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$$
$$display$$yaw'=atan2left ( frac{m_{Y'}}{m_{X'}} right ).$$display$$
Для двух датчиков, которые мы взяли для примера — ADIS16488A и MPU-9250, получены предельные ошибки оценивания углов ориентации при совместном влиянии погрешностей акселерометра и магнитометра.
В таблице ниже — максимальные значения полученных ошибок:
Угол | MPU-9250 | ADIS16488A |
Крен |
$$display$$30^{circ}$$display$$ |
$$display$$8^{circ}$$display$$ |
Тангаж |
$$display$$10^{circ}$$display$$ |
$$display$$2^{circ}$$display$$ |
Магнитный азимут |
$$display$$30^{circ}$$display$$ |
$$display$$20^{circ}$$display$$ |
Совместное влияние погрешностей акселерометра и магнитометра на ошибки оценивания углов ориентации:
Допустим мы поворачиваем ось чувствительности Z ($inline$z1rightarrow z2$inline$) акселерометра так, чтобы проекция силы тяжести на эту ось стала меньше ($inline$g'rightarrow g"$inline$). Значение проекции силы тяжести плюс погрешность акселерометра дадут область возможных значений измерения (розовая область). Погрешность оценки угла при этом возрастает ($inline$Delta _{1}rightarrow Delta _{2}$inline$). Таким образом, при уменьшении проекции вектора силы тяжести на ось чувствительности ошибка акселерометра начинает вносить бОльшую ошибку в оценку угла.
Влияние погрешностей только магнитометра (акселерометр считаем идеальным) на ошибки оценивания углов ориентации:
Автор: Дарья Малафеева
Источник [20]
Сайт-источник PVSM.RU: https://www.pvsm.ru
Путь до страницы источника: https://www.pvsm.ru/matematika/348878
Ссылки в тексте:
[1] Ссылка на гитхаб: https://github.com/DaryaMalafeeva/mems-errors
[2] тут: https://www.nxp.com/docs/en/application-note/AN3461.pdf
[3] Здесь: https://www.nxp.com/docs/en/application-note/AN4248.pdf
[4] Image: https://habrastorage.org/webt/8h/zt/i_/8hzti_e87qh5fpi0n33blb1mofg.jpeg
[5] Image: https://habrastorage.org/webt/8-/se/jj/8-sejjpgdcnjb6av8i0_84uejxe.jpeg
[6] Image: https://habrastorage.org/webt/sa/gz/mo/sagzmoutcde7rcylwyflszcqrkc.jpeg''
[7] Image: https://habrastorage.org/webt/7z/1z/1l/7z1z1lwzpbjo3gx40g9wv6ghhey.jpeg
[8] Image: https://habrastorage.org/webt/y5/vw/lf/y5vwlfpjztzubfrm73mp_7_mna8.jpeg
[9] Image: https://habrastorage.org/webt/wj/tc/_l/wjtc_lxe91pebbeazb2rxwxfs1m.jpeg
[10] Image: https://habrastorage.org/webt/my/gg/td/myggtdgevpkzsraz_rj5s8pefba.jpeg
[11] Image: https://habrastorage.org/webt/ti/_4/ij/ti_4ijkplj_trq3ji5mdvg4ry5m.jpeg
[12] Image: https://habrastorage.org/webt/ot/mo/dr/otmodr4q7nkky7vta-lli0rym4c.jpeg
[13] Image: https://habrastorage.org/webt/6t/yl/lp/6tyllphpw0dyad_g2_tladvti98.jpeg
[14] Image: https://habrastorage.org/webt/ao/d_/uw/aod_uwzhu6uviujwjpmbdndrcxy.jpeg
[15] Image: https://habrastorage.org/webt/1b/gk/x4/1bgkx44gsrheco43ljsjbl1a1os.jpeg
[16] Image: https://habrastorage.org/webt/uy/-j/kt/uy-jkttvi-otrzcdlnizgr7woti.jpeg
[17] Image: https://habrastorage.org/webt/sp/87/k1/sp87k1yxi84ueqesuq1zyih6me4.jpeg
[18] MPU-9250 Product Specification Revision 1.1 — InvenSense Inc.: https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
[19] ADIS16488A Data Sheet — Analog Devices, Inc.: https://www.analog.com/media/en/technical-documentation/data-sheets/ADIS16488A.pdf
[20] Источник: https://habr.com/ru/post/491476/?utm_source=habrahabr&utm_medium=rss&utm_campaign=491476
Нажмите здесь для печати.