👩🏾‍🤝‍👩🏻 ⏺️ ⏮️ Accelerometer MEMS, magnetometer, dan sudut orientasi 🤴🏾 👊 🕵️

Ketika mengembangkan perangkat di mana perlu untuk mengevaluasi sudut orientasi, pertanyaan mungkin timbul - sensor MEMS mana yang harus dipilih. Produsen sensor menyediakan banyak parameter yang darinya kita perlu memperoleh informasi yang berguna tentang kualitas sensor.

Untuk memahami akurasi sudut seperti apa yang dapat kita andalkan, Anda perlu melakukan sejumlah upaya.

TL; DR : Sebuah skrip kecil untuk Oktaf / MATLAB dijelaskan, yang memungkinkan untuk memperkirakan kesalahan dalam menghitung sudut orientasi dari pengukuran MEMS pada akselerometer dan magnetometer. Pada input skrip - parameter sensor dari lembar data (dan / atau kesalahan kalibrasi). Artikel ini mungkin bermanfaat bagi mereka yang mulai menggunakan sensor inersia di perangkat mereka. Program pendidikan kecil pada sensor disertakan. Tautan ke github juga.

Kami segera menerima ketentuan berikut:

Kami ingin mengevaluasi sudut orientasi perangkat stasioner.

Mengapa?
, .
Untuk memperkirakan sudut, kami akan menggunakan pengukuran MEMS dari accelerometer dan magnetometer.

1. Program pendidikan singkat

Sudut orientasi

Kami akan memahami sudut orientasi objek sebagai sudut Euler - roll, pitch, yaw, yang menghubungkan sistem koordinat objek XYZ dan sistem koordinat lokal-utara-utara (ENU - East North Up). Sudut roll, pitch, yaw menunjukkan rotasi yang harus dilakukan sumbu XYZ untuk bergerak di sumbu ENU. Dengan demikian, sudut nol berarti sumbu X objek menghadap ke timur, sumbu Y objek melihat ke utara, dan sumbu Z naik.

Urutan rotasi sumbu adalah dari sudut terakhir: pertama pada yaw (di sekitar sumbu Z), kemudian di pitch (di sekitar sumbu Y), kemudian di roll (di sekitar sumbu X).

Akselerometer

Ini adalah sensor yang mengukur proyeksi akselerasi nyata ke sumbu sensitivitas. Tampaknya - karena mengukur gravitasi juga, bahkan ketika accelerometernya tetap. Sangat mudah untuk membayangkan accelerometer sebagai beban pada pegas, pengukuran yang dikeluarkan sebanding dengan tingkat ketegangan pegas. Jika accelerometer dalam keadaan diam, pegas hanya akan terentang oleh gravitasi. Jika itu mempercepat, maka akan ada sejumlah kekuatan: inersia dari berat

(F = m \vec{a})

$\left (F=m\overrightarrow{a}\right )$ dan gravitasi

(F_{g} = m \vec{g})

$\left (F_{g}=m\overrightarrow{g}\right )$

Kami menerima model pengukuran trias akselerometer ortogonal (saling tegak lurus) berikut:

a_{X Y Z} = m_{a} \cdot A_{X Y Z} + b_{a} + n_{a},

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

Dimana

a_{X Y Z}

$a_{XYZ}$ - Akselerasi terukur dalam SSC (sistem koordinat sendiri) XYZ,

m_{a}

$m_{a}$ - Miringkan matriks sumbu dan faktor skala akselerometer,

A_{X Y Z}

$A_{XYZ}$ Apakah vektor percepatan sejati dalam CCK XYZ,

b_{a}

$b_{a}$ Apakah vektor perpindahan nol dari accelerometer,

n_{a}

$n_{a}$ - Kebisingan pengukuran.

Matriks miring dari sumbu dan faktor skala adalah sebagai berikut:

m_{a} = [\begin{matrix} 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{matrix}],

$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},$

dimana elemen-elemen tersebut berada pada diagonal utama (

1 + m_{a, 1, 1}, 1 + m_{a, 2, 2}, 1 + m_{a, 3, 3}

$1+m_{a,1,1}, 1+m_{a,2,2}, 1+m_{a,3,3}$ ) Apakah faktor skala dan kesalahannya di sepanjang tiga sumbu accelerometer, dan elemen yang tersisa dari matriks adalah distorsi dari sumbu accelerometer.

Pemilihan akselerometer dari lembar data

MPU-9250

— Zero-G Initial Calibration Tolerance ( $\pm60mg$ $X, Y$ , $\pm80mg$ $Z$ ) — $g$ $10^{-3};$
— Initial Tolerance ( $\pm3\%$ ) — . , $10^{-2};$
— Cross Axis Sensitivity ( $\pm2\%$ ) — $10^{-2};$
— Noise Power Spectral Density $\left (300\frac{\mu g}{\sqrt{Hz}} \right )$ — $g$ $10^{-6};$
— Low Pass Filter Response $\left (5-260 Hz\right )$ — , . . ;

σ_{n o i s e} = G_{0} \cdot \sqrt{Π_{n o i s e}};

$\sigma _{noise}=G_{0}\cdot\sqrt{\Pi_{noise}};$

ADIS16488A:

— Bias Repeatability ( $\pm 16mg$ ) — $g$ $10^{-3};$
— (Sensitivity) Repeatability ( $\pm 0.5\%$ ) — ;
— Misalignment Axis to frame ( $\pm1^{\circ}$ ) — , (, );
— Noise Density $\left ( 0.063\frac{mg}{\sqrt{Hz}}rms \right )$ — $g;$
— $\left (3dB Bandwidth\right)$ — , MPU-9250;

Magnetometer

Sebuah sensor yang mengukur proyeksi induksi medan magnet ke sumbu sensitivitas. Magnetometer ditandai oleh distorsi besi-keras dan besi-lunak. Distorsi hard-iron adalah efek aditif ketika komponen konstan ditambahkan ke bidang yang diukur. Alasannya mungkin, misalnya, aksi magnet permanen atau offset nol sensor otomatis. Distorsi soft-iron adalah efek pengganda yang mencerminkan perubahan arah dan / atau pelemahan vektor induksi magnetik. Efek ini dapat disebabkan oleh adanya benda logam di sekitar magnetometer atau oleh distorsi sensor sendiri - kesalahan dalam faktor skala atau distorsi sumbu sensitivitasnya.
Kami menerima model pengukuran triad magnetometer:

m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m},

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

Dimana

m_{X Y Z}

$m_{XYZ}$ - Pengukuran magnetometer dalam CCK XYZ,

S_{m}

$S_{m}$ - matriks miring miring diagonal dan faktor skala (yang menggambarkan efek besi lunak),

M_{X Y Z}

$M_{XYZ}$ Apakah vektor induksi magnetik sejati dalam SSK,

b_{m}

$b_{m}$ - perpindahan nol magnetometer (menjelaskan efek keras - besi),

n_{m}

$n_{m}$ - Kebisingan pengukuran.
Matriks sumbu miring dan faktor skala magnetometer:

S_{m} = [\begin{matrix} 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{matrix}],

$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},$

elemen yang terletak di diagonal utama (

S_{m, 1, 1}, S_{m, 2, 2}, S_{m, 3, 3}

$S_{m,1,1}, S_{m,2,2}, S_{m,3,3}$ ) Apakah faktor skala dan kesalahannya di sepanjang tiga sumbu magnetometer, unsur-unsur yang tersisa dari matriks adalah distorsi dari sumbu magnetometer. Semua elemen matriks juga memperhitungkan efek soft-iron.

Pilihan parameter magnetometer dari lembar data

MPU-9250

, , :

— $(1\mu T);$
— $(5\%);$
— , , — $(\pm2\%);$
— $(0.6\mu T);$

ADIS16488A

— Initial Bias Error $\left (\pm15 mgauss = 1.5 \mu T \right )$ — , $(0.5 \mu T)$ ;
— Initial Sensitivity Tolerance $\left (2\% \right );$
— Misalignment Axis to axis $\left (0.35^{\circ} \right )$ — , (, );
— Noise Density $\left (0.042\frac{mgauss}{\sqrt{Hz}} \right )$ — $\left (\frac{Tesla}{\sqrt{Hz}} \right );$
— $260 Hz;$

Perhitungan sudut orientasi

Karena adanya gravitasi di Bumi, akselerometer "merasakan" arah ke bawah. Pengukuran mereka digunakan untuk menghitung sudut roll dan pitch. Rumus untuk perhitungan dapat ditemukan di sini . Yang ketiga adalah sudut menguap (dan dalam hal ini, azimuth magnetik), dapat ditentukan karena keberadaan medan magnet Bumi. Vektor induksi medan magnet diukur dengan magnetometer dan pengukurannya terlibat dalam perhitungan sudut yaw. Perlu dicatat bahwa dalam menghitung azimuth magnetik, pengukuran magnetometer digunakan, dihitung dalam bidang. Di sini Anda dapat menemukan rumus untuk menghitung azimuth magnetik.

r o l l = a t a n (\frac{a_{Y}}{a_{Z}}),

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

p i t c h = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}),

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

y a w = a t a n 2 (\frac{m_{E}}{m_{N}}),

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

Dimana

a t a n 2

$atan2$ - Fungsi tangen busur penuh,

a_{X}

$a_{X}$ ,

a_{Y}

$a_{Y}$ ,

a_{Z}

$a_{Z}$ - Pengukuran accelerometer sepanjang tiga sumbu di CCK,

m_{E}

$m_{E}$ ,

m_{N}

$m_{N}$ - Pengukuran magnetometer sepanjang sumbu X ', Y' (pengukuran magnetometer dihitung di dalam pesawat).

2.

roll, pitch, yaw. .
?
, — .
roll, pitch, yaw XYZ ENU:

$C_{X Y Z}^{E N U} = | \begin{matrix} c y \cdot c p & - c r \cdot s y + s r \cdot c y \cdot s p & s r \cdot s y + c r \cdot c y \cdot s p \\ s y \cdot c p & c r \cdot c y + s r \cdot s y \cdot s p & - s r \cdot c y + c r \cdot s y \cdot s p \\ - s p & s r \cdot c p & c r \cdot c p \end{matrix} |,$
$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},$
$cr=\cos (roll)$ , $sr=\sin(roll)$ , $cp=\cos(pitch)$ , $sp=\sin(pitch)$ , $cy=\cos(yaw)$ , $sy=\sin(yaw)$ .
:
$A_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot | \begin{matrix} 0 \\ 0 \\ - 1 \end{matrix} |,$
$A_{XYZ}=\left ( C_{XYZ}^{ENU} \right )^{T}\cdot \begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix},$

$\begin{vmatrix} 0\\ 0\\ -1\\ \end{vmatrix}$ — , , g, ${(C_{XYZ}^{ENU} )}^{T}$ — ( ).
:
$a_{X Y Z} = (I + m_{a}) \cdot A_{X Y Z} + b_{a} + n_{a},$
$a_{XYZ}=\left ( I+m_{a} \right )\cdot A_{XYZ}+b_{a}+n_{a},$
() :
$r o l l^{'} = a t a n (\frac{a_{Y}}{a_{Z}}),$
$roll'=atan \left ( \frac{a_{Y}}{a_{Z}} \right ),$
$p i t c h^{'} = a t a n (\frac{- a_{X}}{\sqrt{a_{Y}^{2} + a_{Z}^{2}}}) .$
$pitch'=atan\left ( \frac{-a_{X}}{\sqrt{a_{Y}^{2}+a_{Z}^{2}}} \right ).$
«» , rpy2mat:
$C_{X Y Z}^{X Y Z^{'}} = r p y 2 m a t ({[\begin{matrix} r o l l^{'} \\ p i t c h^{'} \\ 0 \end{matrix}]}^{T}),$
$C_{XYZ}^{XYZ'}=rpy2mat\left ( \begin{bmatrix} roll'\\ pitch'\\ 0 \end{bmatrix}^{T} \right ),$
roll' pitch' — , , — .
ENU XYZ:
$M_{X Y Z} = {(C_{X Y Z}^{E N U})}^{T} \cdot M_{E N U} .$
$M_{XYZ}= {(C_{XYZ}^{ENU} )}^{T}\cdot M_{ENU}.$
:
$m_{X Y Z} = S_{m} \cdot M_{X Y Z} + b_{m} + n_{m} .$
$m_{XYZ}=S_{m}\cdot M_{XYZ}+b_{m}+n_{m}.$
«»:
$m_{X Y Z^{'}} = C_{X Y Z}^{X Y Z^{'}} \cdot m_{X Y Z} .$
$m_{XYZ'}=C_{XYZ}^{XYZ'}\cdot m_{XYZ}.$
«» c :
$y a w^{'} = a t a n 2 (\frac{m_{Y^{'}}}{m_{X^{'}}}) .$
$yaw'=atan2\left ( \frac{m_{Y'}}{m_{X'}} \right ).$
roll, pitch, yaw — roll', pitch', yaw'.

3. —

Untuk dua sensor, yang kami ambil sebagai contoh - ADIS16488A dan MPU-9250, kesalahan marginal dalam estimasi sudut orientasi diperoleh di bawah pengaruh gabungan dari kesalahan accelerometer dan magnetometer.

Tabel di bawah ini menunjukkan nilai maksimum kesalahan yang diterima:

Sudut	MPU-9250	ADIS16488A
Gulungan	$30^{\circ}$ $30^{\circ}$	$8^{\circ}$ $8^{\circ}$
Nada	$10^{\circ}$ $10^{\circ}$	$2^{\circ}$ $2^{\circ}$
Azimuth magnetik	$30^{\circ}$ $30^{\circ}$	$20^{\circ}$ $20^{\circ}$

Efek gabungan dari kesalahan accelerometer dan magnetometer pada kesalahan estimasi sudut orientasi:

Ini adalah bagaimana kesalahan estimasi roll terlihat tergantung pada nilai roll dan pitch:
Kesalahan estimasi pitch tergantung pada nilai roll dan pitch:
Kesalahan dalam mengestimasi azimuth magnetik dari sudut roll and pitch:
Kesalahan estimasi azimuth magnetik dari sudut roll dan azimuth magnetik:
Kesalahan dalam mengestimasi azimuth magnetik dari sudut pitch dan azimuth magnetik: