BAB II DASAR TEORI 2.1 Quadrotor Quadrotor merupakan salah satu jenis wahana yang dapat terbang dan mendarat secara vertikal. Kemampuan tersebut didapatkan dengan menggunakan 4 buah motor dan baling-baling sebagai aktuator. Berdasarkan peletakan motor, secara umum quadrotor terdiri dari empat jenis frame, yaitu seperti pada gambar dibawah ini:
Gambar 2.1 Jenis frame quadcopter berdasarkan posisi motor Quadrotor dengan frame jenis plus-frame dan x-frame merupakan jenis frame yang memiliki jarak simetris antara motor ke motor dan jarak simetris antara motor ke titik pusat quadrotor. Sedangkan jenis H-frame dan Y-frame penempatan masing-masing motor yang tidak simetris. Quadrotor jenis X-frame digunakan dalam pembahasan ke depan
6
7
2.1.1
Koordinat Quadrotor Secara umum quadrotor menggunakan sistem koordinat yang sama dengan
sistem koordinat pada pesawat terbang. Sistem koordinat pada pesawat terbang dapat diilustrasikan pada gambar 2.2.
Gambar 2.2 Koordinat sudut Yaw, Pitch dan Roll pada Pesawat F161 Sistem koordinat yang digunakan melibatkan dua buah sistem koordinat terpisah. Sistem koordinat pertama adalah sistem koordinat bumi, dimana sistem koordinat ini merupakan sistem koordinat kartesian relatif terhadap permukaan bumi. Sistem koordinat kedua adalah sistem koordinat quadrotor, dimana sistem koordinat ini merupakan sistem koordinat pada rangka quadrotor. Dengan menetapkan kedua sistem tersebut, maka sudut euler dapat ditentukan, dimana sudut euler yang digunakan merupakan konvensi rotasi z-y-x atau dapat disebut sudut Tait-Bryan, yaitu sudut yaw (ѱ), pitch (θ) dan roll (ϕ).
1
http://dle-tech.info/tag/pitch/
8
Gambar 2.3 Koordinat bumi dan koordinat quadrotor Pada gambar 2.3 menunjukan perbedaan koordinat bumi dan koordinat pada quadrotor. Sudut Tait-Bryan yang membentuk rotasi sebuah objek dalam ruang (ℝ3 ). Sehingga dibutuhkan 3 parameter untuk merepresentasikan orientasi sebuah objek pada sistem 3 dimensi, sudut-sudut itu antara lain:
rotasi pada sumbu x, disebut dengan roll (ϕ)
rotasi pada sumbu y, disebut dengan pitch (θ)
rotasi pada sumbu z, disebut dengan yaw (ѱ) Dalam kaitannya dengan rotasi matrik, ketiga sudut Tait-Bryan ini dapat
membentuk rotasi matrik pada persamaan (2.1): cos 𝜃 cos 𝜓 𝑅 = [ cos 𝜃 sin 𝜓 − sin 𝜃
sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 sin 𝜙 cos 𝜃
cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓 cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓] cos 𝜙 cos 𝜃
(2.1)
Dimana rotasi matriks dapat direpresentasikan pada persamaan (2.2) 𝑎11 𝑎 𝑅 = [ 21 𝑎31
𝑎12 𝑎22 𝑎32
𝑎13 𝑎23 ] 𝑎31
(2.2)
Sehingga hubungan rotasi matriks dengan sudut Tait-Bryan adalah sebagai berikut:
9
𝑎 𝑎𝑡𝑎𝑛( 21⁄𝑎11 ) 𝜓 [ 𝜃 ] = [ − 𝑎𝑠𝑖𝑛(𝑎31 ) ] 𝑎 𝜙 𝑎𝑡𝑎𝑛( 32⁄𝑎33 )
2.1.2
(2.3)
Sistem Gerak Quadrotor Pada gambar 2.4, menunjukan bahwa 2 buah baling-baling berputar searah
jarum jam, dan 2 baling-baling lainnya berputar berlawanan arah jarum jam. Pada skema motor yang dipakai, penamaan motor dilakukan dengan posisi depan motor 1 dan 2 dan posisi belakang motor 3 dan 4.
Gambar 2.4 Arah putar motor pada quadrotor frame X Perbedaan arah putar tersebut menyebabkan torsi balik dari motor 1 dan motor 4 akan dihilangkan oleh torsi balik dari motor 2 dan motor 4. Konfigurasi ini memungkinkan dihilangnya tail-rotor yang diperlukan dalam konfigurasi tradisional. Pada gambar 2.5 adalah bentuk mekanisme quadrotor pada rangka jenis xframe. Quadrotor bergerak dengan memanfaatkan perbedaan kecepatan motor (𝜔1, 𝜔2 , 𝜔3 dan 𝜔4 ) dengan baling-baling menghasilkan gaya angkat (𝐹1 , 𝐹2 , 𝐹3 dan 𝐹4 ).
10
Gambar 2.5 Mekanisme quadrotor pada x-frame Pada persamaan (2.4) adalah gaya angkat total yang harus dihasilkan oleh motor agar quadrotor dapat terbang. Persamaan (2.5) dan (2.6) juga menunjukan quadrotor untuk naik atau turun. [10] 𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 = 𝑊 = 𝑚 × 𝑔 = 𝐹𝑇
(2.4)
𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 > 𝑊
(2.5)
𝐹1 + 𝐹2 + 𝐹3 + 𝐹4 < 𝑊
(2.6)
Bila quadrotor ingin menghadap ke kiri, hanya perlu merubah gaya angkat pada motornya, yaitu menaikan gaya angkat motor pada depan kiri dan belakang kanannya. Dan juga sebaliknya ketika ingin menghadap ke kanan. Gerakan ini dapat ditulis pada persamaan (2.7). [10] Yaw : 𝜔1 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔2 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔3 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔4 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡
(2.7)
11
Untuk bergerak maju, maka quadrotor perlu menaikan gaya angkat motor di belakang dan menurunkan gaya angkat motor di depannya. Dan juga sebaliknya untuk bergerak mundur dengan menaikan gaya angkat motor di depannya dan menurunkan gaya angkat motor di belakangnya. Gerakan ini dapat ditulis pada persamaan (2.8). [10] Pitch : 𝜔1 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔2 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡 (2.8) 𝜔3 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔4 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 Gerakan manuver ke kiri atau ke kanan, yaitu dengan menaikan gaya angkat motor di kanannya dan menurunkan gaya angkat motor di kirinya untuk manuver ke kiri. Dan juga sebaliknya untuk manuver ke kanan. [10] Roll : 𝜔1 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔2 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡 (2.9) 𝜔3 = 𝐹𝑇 + 𝑜𝑓𝑓𝑠𝑒𝑡 𝜔4 = 𝐹𝑇 − 𝑜𝑓𝑓𝑠𝑒𝑡 Pada gambar 2.6 merupakan keseluruhan gerakan-gerakan quadrotor dengan frame-X dengan kombinasi beberapa gaya angkat motor.
12
Gambar 2.6 Gerakan quadrotor 2.2 Sensor Inersia Inersia atau kelembaman adalah kecenderungan semua benda fisik untuk menolak perubahan terhadap keadaan geraknya. Sensor inersia merupakan suatu alat yang dapat mengukur inersia. Sensor inersia secara umum terdiri dari dua jenis, yaitu accelerometer dan gyroscope 2.2.1
Accelerometer Accelerometer adalah sensor yang digunakan untuk mengukur percepatan
suatu objek sehingga dapat mendeteksi adanya perubahan posisi device/perangkat dan berapa banyak perubahan itu terjadi. Sensor ini dipasang bersama benda yang akan diukur akselerasinya, seperti perubahan kecepatan roket yang meluncur atau digunakan untuk analisis getaran (Vibration analysis) pada mesin, serta digunakan untuk mendeteksi gerak dan kemiringan pada smartphone.
13
Accelerometer pada quadrotor, digunakan sebagai pendeteksi arah gravitasi bumi yang dapat diolah menjadi posisi sudut kemiringan quadrotor terhadap horizontal bumi. 2.2.1.1 Konsep Accelerometer Sistem Massa-Pegas Accelerometer dapat dianalogikan sebagai sebuah sistem massa-pegas (mass spring system) yang bekerja berdasarkan Hukum Newton dan Hukum Hooke. Prinsip kerja dari sensor ini akan dijelaskan sebagai berikut:
a
m1
m1 m2
m2 x0
x0 a) Sistem dalam keadaan normal
x
x1
b) Pegas meregang karena pengaruh akselerasi a
Gambar 2.7 Sistem mass pegas sebagai accelerometer Hukum Newton II menyatakan jiika massa m dan mengalami percepatan sebesar α, maka ada gaya yang bekerja pada massa tersebut sesuai dengan persamaan (2.10). 𝐹 = 𝑚𝑎
(2.10)
Hukum Hooke menyatakan jika pegas dengan konstanta pegas k direnggangkan sehingga berubah panjangnya sebesar ∆x, maka ada gaya F yang bekerja pada pegas tersebut dinyatakan dalam persamaan (2.11). 𝐹 = −𝑘 ∆𝑥
(2.11)
Pada gambar 2.7, diilustrasikan sebuah sistem dengan massa m1 yang bebas bergerak secara horisontal pada sebuah bidang berdinding m2. Massa m1
14
dihubungkan ke dinding bidang m2 oleh sebuah pegas. Awalnya bidang m2 diam (gambar 2.8a) dan pegas dalam kondisi tidak merenggang. Pada gambar 2.7b, ada percepatan horisontal α yang bekerja pada sistem ini menyebabkan pegas merenggang sebesar ∆x. Renggangnya pegas ini dikarenakan adanya gaya yang bekerja pada m1 akibat percepatan α. Dengan menggabungkan Hukum Newton dan Hukum Hooke didapatkan: 𝑚𝑎 = 𝑘 ∆𝑥
(2.12)
Dimana, k = konstanta pegas (N/m) ∆x = perenggangan pegas (m) a = akselerasi sistem (m/s2) Sehingga percepatan yang dialami oleh sistem sebesar: 𝑎 =
𝑘 ∆𝑥 𝑚
(2.13)
Dengan persamaan (2.12), jika konstanta pegas dan massa diketahui, alat ukur percepatan dapat dibuat hanya dengan mengukur perubahan panjang dari pegas. 2.2.1.2 Accelerometer MEMS Sensor accelerometer elektronik adalah sensor accelerometer yang hasil pengukuran akselerasinya dinyatakan dalam tegangan atau data digital. Seperti dijelaskan sebelumnya bahwa accelerometer dapat dibangun dengan massa yang dikaitkan dengan pegas, accelerometer elektronik memiliki prinsip yang sama dalam mengukur percepatan. Hanya saja tidak mungkin untuk membuat sensor dengan ukuran yang relatif besar seperti Gambar 2.7 hingga pada akhir abad 20
15
dikembangkan teknologi MEMS (Micro-Electro-Mechanical System), yang mampu menerapkan prinsip accelerometer massa-pegas ke dalam sebuah chip. Accelerometer dengan teknologi MEMS memanfaatkan perubahan kapasitansi dua buah plat terhadap perubahan jarak antar plat tersebut karena pengaruh akselerasi dari luar.
Gambar 2.8 Topologi accelerometer Pada gambar 2.8 menunjukkan topologi dari accelerometer yang dirancang, yang merupakan lateral accelerometer. Ada dua simetris mata ganda dilipat yang menghubungkan massa (proof mass) untuk jangkar titik tetap (anchor point). Ketika ada eksternal percepatan, proof mass bergerak sepanjang garis melewati titik jangkar ke arah berlawanan dengan percepatan yang diberikan. Gerakan ini menyebabkan kapasitansi diferensial berbeda dalam skema sederhana penginderaan kapasitansi secara melintang. Rangkaian pembacaan eksternal kapasitansi diferensial ini berbeda dan memberikan tegangan output yang berkaitan dengan masukan percepatan. Springs dan proof mass adalah elemen utama yang menentukan resonansi frekuensi accelerometer. Bagian massa biasanya dipilih sebanyak mungkin
16
selama masih bisa diterima oleh proses yang ada. Jadi, untuk mencapai frekuensi resonansi yang diinginkan, desainer biasanya menetapkan massa hingga nilai maksimum dan hanya mengatur nilai konstanta pegasnya. Alasan untuk menggunakan folded springs adalah untuk mencapai frekuensi resonansi rendah, sensitivitas yang lebih tinggi, dan juga untuk mengurangi total tegangan agar di bawah guncangan eksternal.
Gambar 2.9 Foto chip accelerometer dengan teknologi MEMS Dalam
beberapa
desain,
double-folded
springs
digunakan
untuk
mewujudkan frekuensi resonansi yang lebih rendah dan nilai-nilai stres yang lebih rendah. Sementara beberapa accelerometer memiliki yang lebih tinggi untuk aplikasi double-folded springs memiliki folded-springs konvensional untuk mencapai untuk aplikasi bandwidth yang lebar. 2.2.1.3 Percepatan Statis dan Dinamis pada Accelerometer Accelerometer dapat mengukur dua jenis percepatan, yaitu percepatan dinamis dan statis. Percepatan dinamis adalah percepatan objek yang bergerak, disebabkan oleh pergerakan atau getaran. Sedangkan percepatan statis adalah pengukuran percepatan yang dialami oleh benda diam.
17
Setiap benda yang berada dalam medan gravitasi bumi mendapatkan gaya tarik ke pusat bumi atau disebut gaya berat. Sebagai contoh, meskipun batu pada permukaan tanah kelihatan diam, tetapi ada percepatan statis yang bekerja pada batu tersebut karena pengaruh gaya tarik bumi. Ketika mengukur percepatan dengan menggunakan accelerometer, perlu ditilik apakah percepatan statis atau dinamis yang bekerja pada sensor ini, karena hasil pengukuran accelerometer merupakan gabungan antara kedua percepatan in. misalnya benda yang mula-mula diam dan bergerak mendatar terhadap permukaan bumi. Benda tersebut mendapatkan dua percepatan, yaitu percepatan gerak (dinamis) dan percepatan gravitasi yang arahnya ke bawah (statis).
a) Pegas dalam keadaan normal
a b) Pegas meregang karena ada percepatan dimamis (a)
c) Pegas mengerut karena ada percepatan statis (gravitasi)
g
Gambar 2.10 Percepatan dimanis dan statis pada mass-pegas
18
Pada gambar 2.10 diperlihatkan accelerometer massa pegas untuk menjelaskan fenomena ini. Jika kaselerometer massa pegas ini digerakkan searah tanda panah pada accelerometer (Gambar 2.10a) maka pegas akan merenggang karena ada pengaruh gaya yang menimbulkan percepatan. Dengan asumsi bahwa ada gaya gravitasi yang bekerja menuju pusat bumi, jika accelerometer kita arahkan ke tanah (arah panah pada sensor menunjuk ke pusat bumi, (Gambar 2.10b) pegas tidak akan merenggang tetapi mengerut. Respon pegas terhadap percepatan statis (gaya berat) berlawanan dengan respon pegas terhadap percepatan dinamis meskipun kedua percepatan memiliki arah yang sama. 2.2.1.4 Sumbu Pengukuran Accelerometer Dalam mengukur percepatan, accelerometer memiliki sumbu pengukuran (axis). Percepatan yang searah dengan sumbu ini memiliki alat maksimum, tetapi jika arah percepatan ini membentuk sudut maka besarnya percepatan yang terukur merupakan proyeksi percepatan yang bekerja terhadap sumbu pengukuran.
a
Sumbu pengukuran
Percepatan dari luar membentuk sudut α
a cos
Percepatan yang dirasakan oleh akselerometer
akselerometer
Gambar 2.11 Pengukuran percepatan yang membentuk sudut terhadap sumbu pengukuran Gambar
2.11
menunjukkan
proyeksi
percepatan
dinamis
pada
accelerometer yang hanya memiliki satu sumbu pengukuran. Accelerometer yang banyak tersedia biasanya memiliki lebih dari satu sumbu pengukuran yang saling
19
tegak lurus. Sumbu pengukuran ini sama dengan sumbu pada sistem koordinat kartesian.
Gambar 2.12 Orientasi sumbu accelerometer dan gyroscope Jumlah dari sudut pengukuran accelerometer menentukan kapabilitas dari sensor ini. Untuk mendeteksi besar dan arah percepatan pada satu dibutuhkan dua sumbu pengukuran, dan jika dalam ruang dibutuhkan tiga sumbu pengukuran. Pada pengaplikasian dalam AHRS, digunakan accelerometer 3 sumbu pengukuran untuk mendeteksi arah percepatan gravitasi dalam ruang. Karena percepatan gravitasi merupakan percepatan statis, maka sumbu pengukuran harus disesuaikan, mengingat arah percepatan berbanding terbalik dengan percepatan dinamis jika diukur oleh accelerometer. Sehingga accelerometer pada gambar 2.13a memiliki sumbu pengukuran dinamis dan statis seperti gambar 2.13b. X
Z Y
Y X a) sumbu pengukuran dinamis
Z b) sumbu pengukuran statis
Gambar 2.13 Sumbu pengukuran statis dan dinamis pengukuran accelerometer
20
2.2.1.5 Parameter Accelerometer Ada beberapa parameter penting yang dimiliki accelerometer yang tersedia di pasar. Parameter ini penting untuk diperhatikan dalam memilih tipe accelerometer untuk diaplikasikan dalam sebuah sistem. 1. Jumlah sumbu pengukuran (axis) Seperti yang telah dibahas sebelumnya, bahwa jumlah sumbu pengukuran ini menentukan kapabilitas accelerometer dalam mengukur besar dan arah percepatan. Sebagai contoh accelerometer ADXL202 produksi Analog Device memiliki dua sumbu pengukuran, sedangkan LIS3LV02DL produksi ST Microelectronic memiliki tiga sumbu pengukuran. 2. Nilai skala penuh (full scale) Nilai skala penuh merupakan percepatan maksimum yang dapat diukur oleh accelerometer. Nilai skala penuh biasanya mempunyai jangkauan positif dan negatif. Parameter ini penting untuk menentukan efektif tidaknya accelerometer yang akan dipilih terhadap aplikasi dimana accelerometer digunakan. Sebagai contoh, untuk mengukur akselerasi roket dengan percepatan maksimum 60g (588 m/s2, 1g = 9.8 m/s2) akan sangat tidak efektif jika menggunakan ADXL202 yang memiliki skala penuh ±10g. 3. Antarmuka Dalam pengaplikasiannya accelerometer dihubungkan ke unit pemroses seperti mikrokontroler atau mikroprosesor. Ada dua jenis sistem antarmuka accelerometer untuk berhubungan dengan unit pengolah, yaitu analog dan digital. Pada antarmuka analog, hasil pengukuran percepatan direpresentasikan dalam tegangan keluaran sedangkan pada antarmuka digital percepatan hasil pengukuran
21
direpresentasikan dengan data digital. Accelerometer dengan antarmuka digital menggunakan protocol komunikasi yang banyak dipakai dalam sistem benam seperti 12C (Inter-Integrated Circuit), SPI (Serial Peripheral Interface) dan PWM (Pulse Width Modulation). Di dalam accelerometer dengan keluaran data digital sudah terdapat ADC (Analog Digital Converter) internal sehingga tidak diperlukan lagi ADC tambahan. 4. Frekuensi cuplik Besarnya
frekuensi
cuplik
accelerometer
merupakan
kemampuan
accelerometer untuk memperbarui data percepatan dalam periode waktu tertentu. Parameter ini penting untuk diperhitungkan pada aplikasi accelerometer untuk mengukur jarak atau navigasi. 2.2.2 Gyroscope Gyroscope adalah sensor yang dapat mengukur kecepatan angular dari sebuah objek dimana sensor ini terpasang. Sensor ini sering digunakan pada sistem navigasi pesawat untuk menentukan orientasi arah. 2.2.2.1 Prinsip Kerja Gyroscope Garpu Tala Ada banyak metode untuk mendeteksi kecepatan sudut, antara lain vibrating ring gyroscope, tuning fork gyroscope, macro laser ring gyroscope dan piezoelectric plate gyroscope. Metode yang paling banyak digunakan dan diproduksi sampai sekarang adalah gyroscope garpu tala Draper (Draper tuning fork). Gyroscope garpu tala dibuat dengan memanfaatkan resonansi dari dua buah resonantor yang bergetar yang disebabkan oleh efek Coriolis. Efek Coriolis
22
adalah defleksi yang timbul pada kerangka acuan rotasi yang besarnya berbanding lurus dengan kecepatan rotasi. Fenomena ini dijelaskan sebagai berikut:
Gambar 2.14 Bola yang diluncurkan pada piringan yang berputar Jika ada sebuah bola pada pusat sebuah piring besar yang dapat berputar seperti pada gambar 2.14. Saat piringan besar tersebut tidak berputar dan bola yang diluncurkan dari pusat sebuah piring, pada umumnya bola tersebut bergerak lurus dari pusat piring. Akan tetapi jika pada piring besar tersebut berputar dan bola diluncurkan, maka bola tersebut tidak memiliki lintasan lurus (seperti saat piring besar tidak berputar) tetapi berbelok. Hal ini disebabkan karena adanya pengaruh rotasi piring terhadap gerak dari bola. Semakin cepat piring berputar, semakin besar pula pembelokan peluru yang terjadi. Fenomena inilah yang disebut dengan efek Coriolis.
23
Gambar 2.15 Efek Coriollis pada gyroscope garpuatala Pada gambar 2.15, jika ada benda bergerak dengan kecepatan 𝑣̅ searah sumbu y, dan mendapat pengaruh rotasi dengan kecepatan sudut 𝜔 ̅ pada sumbu z maka akan timbul akselerasi Coriollis 𝛼̅𝑐𝑜𝑟 yang searah dengan sumbu x; 𝛼̅𝑐𝑜𝑟 = 2𝑣̅ × 𝜔 ̅
(2.14)
MEMS Gyroscope dibangun berdasarkan prinsip Coriolis pada sebuah garpu tala yang bergetar pada gambar 2.16. jika garpu tala digetarkan pada sumbu y dan garpu tala tersebut diputar pada sumbu z, maka dengan prinsip Coriolis akan timbul getaran yang dirasakan pada sumbu x. selanjutnya dengan rangkaian elektronik getaran pada sumbu x ini dikonversikan ke dalam besaran elektrik sehingga kecepatan putar dapat dengan mudah diukur dan diolah. Dengan teknologi MEMS, sangat dimungkinkan untuk membuat gyroscope dalam ukuran yang sangat kecil, meskipun di dalamnya terdapat sistem mekanik yang rumit. Gambar menunjukkan foto mekanik gyroscope garpu tala Draper dengan teknologi MEMS.
24
Gambar 2.16 MEMS gyroscope pada iPhone 4 Seiring dengan berkembang pesatnya teknologi MEMS, membuat MEMS Gyroscope menjadi semakin kecil, kompak dan murah. Banyak produsen komponan elektronik yang mengembangkan MEMS gyro ini, diantaranya adalah ST Microelectronics, Analog Device dan InvenSense. 2.2.2.2 Sumbu Pengukuran Gyroscope Sama halnya dengan
accelerometer, gyroscope memiliki sumbu
pengukuran. Rotasi dideteksi berdasarkan sumbu pengukuran yang menjadi poros rotasi. Ada banyak gyroscope yang tersedia di pasar dan beberapa diantaranya memiliki lebih dari satu sumbu yang artinya ada lebih dari satu gyroscope dalam sebuah chip. Sumbu-sumbu rotasi pengukuran tersebut saling tegak lurus sehingga posisi masing-masing gyroscope didalamnya juga saling tegak lurus. 2.2.2.3 Parameter Sensor Gyroscope Sensor gyroscope memiliki beberapa parameter yang menentukan karakteristik dan kualitas dari sensor ini:
25
1. Resolusi Resolusi ini merupakan kecepatan putar minimum yang dapat dideteksi oleh sensor. Pada gyroscope dengan keluaran data digital, resolusi dinyatakan dalam satuan bit terkecil per kecepatan putar atau LSB (o/s). gyroscope dengan resolusi tinggi dapat mendeteksi perubahan orientasi yang kecil. 2. Full-Scale Range Full-Scale Range merupakan jangkauan maksimum besarnya kecepatan putar yang dapat dideteksi oleh sensor. Sebagai contoh, sensor gyroscope ITG3205 memiliki full scale range sebesar ±2000 o/s. Artinya, sensor ini dapat mendeteksi kecepatan putar maksimum 2000o dalam satu detik atau 34.8894 rad/s. 3. ZRO (Zero Rate Output) Zero Rate Output pada sensor gyroscope merupakan keluaran sensor saat diam (tidak berotasi). Dalam implementasinya untuk mengukur arah hadap, yaitu dengan mengintegralkan kecepatan sudut (keluaran gyroscope), keluaran gyroscope harus di-offset dengan ZRO-nya terlebih dahulu agar nilai ZRO ini tidak ikut diintegralkan dari waktu ke waktu. 4. Short-or-Long term Drift Pada saat diam, meskipun sudah di-offset dengan ZRO, data keluaran sensor tidak akan tetap 0o/s, tapi berubah-ubah. Perubahan ini kecil dan dengan frekuensi yang lambat, akan tetapi sangat terasa nantinya jika diintegralkan dalam jangka waktu yang lama. Short-or Long-term Drift merupakan nilai peak-to-peak dari keluaran gyroscope saat tidak ada rotasi. 5. Jumlah sumbu pengukuran
26
Ada banyak gyroscope yang diproduksi dan memiliki lebh dari satu sumbu pengukuran. Misalnya LPY503AL produksi ST Microelectronics memiliki 2 sumbu pengukuran dan ITG3205 produksi InvenSense memiliki 3 sumbu pengukuran. Jumlah sumbu pengukuran menentukan kapabilitas dari gyroscope dalam mendeteksi rotasi. Misalnya, gyroscope dengan hanya dua sumbu pengukuran tidak cukup untuk menentukan arah hadap dalam ruang. Untuk mengukur rotasi atau arah hadap dalam ruang dibutuhkan tiga sumbu pengukuran. 2.3 Sensor Magnetometer Magnetometer adalah sebuah instrumen pengukuran yang digunakan untuk dua tujuan umum - untuk mengukur magnetisasi bahan magnetik seperti feromagnet, atau untuk mengukur kekuatan dan, dalam beberapa kasus, arah medan magnet pada suatu titik dalam ruang angkasa (juga dikenal sebagai Gaussmeter atau magnetometer survei). Magnetometer pertama kali ditemukan oleh Carl Friedrich Gauss pada tahun 1833 dan perkembangan penting dalam abad ke-19 termasuk Hall Effect yang masih banyak digunakan. 2.3.1
Sensor Fluxgate Sensor fluxgate mengukur nilai x arah dari medan magnet dc atau
frekuensi rendah dengan range sekitar 10-10 hingga 10-4 T. Prinsip dasar diilustrasikan pada gambar 2.5.1. Material magnetic lunak inti sensor secara periodic tersaturasi oleh kedua medan eksitasi, dimana hal tersebut menghasilkan arus eksitasi melalui koil eksitasi. Karena hal tersebut permeabilitas berubah dan flux yang berasosiasi dengan medan magnet DC B0 termodulasi.
27
Gambar 2.17 Prinsip dasar fluxgate 2.3.2
Magnetoresistor Sensor magnetoresistor berubah nilai resistansinya sesuai dengan
kecepatan fluks magnetik. Alat ini terbuat dari nickel-iron(Permalloy) yang tertanam pada lapisan tipis pada permukaan semikonduktor. Hal ini membutuhkan fabrikasi khusus yang mengkonduksikan lapisan pada semikonduktor yang mudah bergerak seperti Indium-Antimonide atau Indium Arsenide. Lapisan tipis tertanam pada medan magnet yang kuat dimana mengorientasikan magnetisasi M pada arah parallel dengan resistor. Arus lalu dibuat melewati lapisan tipis pada sudut θ pada arah M. Jika sudut nol, lapisan tipis akan memiliki resistansi tertinggi. Pada sudut θ, resistansi yang terjadi adalah terendah. Ketika medan magnet eksternal diaplikasikan secara sejajar kepada M, maka θ berubah dan resistansi berubah. Prinsip dasar ini menghasilkan perubahan resistansi ketika medan magnet diaplikasikan dan memperbolehkan lapisan tipis digunakan sebagai sensor seperti pada gambar 2.5.2.
Gambar 2.18 Magneto resistor
28
Sensor magnetoresistive sangat sesuai untuk kekuatan magnet sedangkan dengan contoh navigasi medan bumi dan sistem ukur, karena 2.4 Sensor Barometer Barometer merupakan alat yang dapat mengukur tekanan udara. Hal ini memungkinkan peramal cuaca atau ilmuan dapat memprediksi cuaca lebih akurat hingga cuaca ekstrim. Jika tekanan udara yang terukur tinggi menunjukkan cuaca yang bersahabat, namun sebaliknya jika yang terukur tekanan rendah, memungkinkan terjadi badai. Istilah barometer diperkenalkan sekitar tahun 1640 -1643 oleh seorang ilmuwan asal Iralndia bernama Robert Boyle. Nama barometer berasal dari bahasa Yunani yang terdiri dari kata Baros yang berarti berat/bobot dan kata Metron yang berari ukuran. Evangellista Torricelli, merupakan ilmuwan yang dinobatkan sebagai penemu barometer ditahun 1643. Meskipun banyak yang menyatakan bahwa barometer itu sendiri awalnya bukan ditemukan oleh Torricelli. Tahun 1630, Giovanni Battista Baliani memberikan surat perintah untuk melakukan percobaan kepada Galileo Galilei di atas bukit setinggi 21 meter. namun percobaannya gagal. Ia menjelaskan bahwa terdapat tekanan vakum pada air. pada ketinggian tertentu, jumlah titik didih air menjadi lebih tinggi dan tekanan udara lebih rendah. Seperti halnya seutas tali yang menahan banyak berat badan, Sehingg percobaannya mengalami kegagalan dalam membuat barometer. Berita tersebut tersebar luas, sehingga dari Galileo sampai kepada Aristoteles dan berakhir di Toriccelli yang juga teman sekelas Galileo. Torricelli mempertanyakan asumsi dari Aroistoteleh dan galileo bahwa udara tidak memiliki
29
berat lateral. Menurut Toricelli vakum memiliki berat sehingga ia mampu mendorong kolom air. Kemudian Torricelli dibantu saran dari Galileo, agar percobaannya berhasil, ia harus menggunakan cairan yang lebih berat dari air. cairan merkuri atau yang kita serinbg sebut air raksa adalah solusi yang tepat. Ia memiliki kepadatan lebih baik dari air, sekitar 14 kali lebih berat dari air. Percobaannya berhasil dan ia dinobatkan sebagai penemu alat ukur tekanan yang disebut barometer, dan kemudian disempurnakan oleh Blaise Pascal pada tahun 1646.
Gambar 2.19 Bagan barometer air raksa Meskipun tampak sederhana dengan manfaatnya yang luar bisa, untuk menciptakan barometer tak semudah dibayangkan. Prosesnya panjang hingga
30
mencapai paripurna dalam keakuratan mengukur tekanan udara dan prakiraan cuaca. Terlebih prinsip kerja dari barometer raksa mengacu pada konsep fisika tentang fluida terutama bersangkutan dengan tekanan, hukum kontinuitas dan manometer. Sebuah barometer raksa memiliki tabung kaca dengan ketinggian minimal 84 cm, ditutup pada salah satu ujungnya, dengan reservoir merkuri mengisi penuh, dan terbuka di pangkalnya. Berat merkuri menciptakan vakum di bagian atas tabung. Merkuri dalam tabung menyesuaikan sampai berat merkuri dalam kolom tabung menghasilkan tekanan atmosfer bekerja pada reservoir. Barometer bekerja dengan menyeimbangkan berat merkuri dalam tabung gelas terhadap tekanan atmosfer sama seperti satu set timbangan. Jika berat merkuri kurang dari tekanan atmosfer, tingkat merkuri dalam tabung gelas naik. Jika berat merkuri lebih dari tekanan atmosfer, tingkat merkuri jatuh/turun. Tekanan atmosfer pada dasarnya adalah berat udara di atmosfer di atas reservoir, sehingga tingkat merkuri terus berubah sampai berat merkuri dalam tabung gelas persis sama dengan berat udara di atas reservoir. Torricelli mencatat bahwa ketinggian air raksa dalam barometer berubah sedikit setiap hari dan menyimpulkan bahwa ini dikarenakan terjadi perubahan tekanan di atmosfer. Desain barometer merkuri yang menimbulkan ekspresi tekanan atmosfir dalam inci atau milimeter atau kaki: tekanan dikutip tingkat tinggi merkuri dalam kolom vertikal. Biasanya, tekanan atmosfer diukur antara 26.5-31.5 inci Hg.
31
Prinsipnya tekanan atmosfer normal (1 atm) adalah setara dengan 760 milimeter air raksa. 2.4.1
Barometer Air Raksa Barometer air raksa tersedia dalam berbagai desain, barometer raksa
standar terdiri dari tabung kaca vertikal dengan kolom merkuri di dalamnya. Ujung atas tabung kaca disegel (tertutup), sedangkan ujung tabung yang lain dibiarkan terbuka dan dibenamkan dalam wadah yang berisi air raksa.
Gambar 2.20 Foto barometer air raksa Ketika tekanan atmosfer turun, kolom merkuri dalam tabung kaca juga turun, fenomena yang menandakan potensi badai. Saat badai berlalu, level merkuri akan mulai naik seiring dengan tekanan atmosfer yang juga naik. 2.4.2
Barometer Air Barometer air juga dikenal sebagai termometer Goethe, terdiri dari wadah
kaca tertutup yang setengah terisi air dan semacam cabang kecil (cerat). Cerat kaca terhubung ke wadah kaca. Karena saling terhubung, cerat dan wadah kaca akan terisi air.
32
Gambar 2.21 Foto barometer air Ketika tekanan atmosfer rendah, level air pada cerat perlahan akan naik melebihi permukaan air dalam wadah kaca. Bila level air di cerat turun, hal ini berarti tekanan atmosfer berubah menjadi lebih tinggi. 2.4.3
Barograf Barograf merupakan jenis barometer aneroid, namun tidak hanya
melakukan pembacaan melainkan juga merekam hasil pencatatan selama periode waktu tertentu. Dibuat dari silinder logam dengan lengan pena, barograf membaca perubahan tekanan atmosfer seperti barometer aneroid sedangkan lengan pena mencatat hasil pengukuran pada kertas atau media lain.
Gambar 2.22 Foto barograf
33
Hasil pencatatan ini dikenal sebagai barogram, memungkinkan para ilmuwan dan ahli meteorologi untuk mempelajari iklim suatu daerah dalam jangka panjang, bukan hanya cuaca dalam satu hari. 2.4.4
Barometer Aneroid Diciptakan pada tahun 1843, barometer aneroid memiliki mekanisme yang
rumit untuk membaca perubahan tekanan atmosfer. Barometer aneroid terdiri dari wadah dan semacam logam lentur yang dikenal sebagai kapsul aneroid atau sel.
Gambar 2.23 Foto barometer aneroid Aneroid ini terbuat dari paduan berilium dan tembaga. Wadah kemudian disegel setelah udara dikosongkan. Ketika kotak logam mengembang atau menyusut karena perubahan tekanan luar, perangkat dalam barometer menerjemahkannya menjadi pembacaan tekanan udara. 2.4.5
Barometer Digital Barometer digital terdiri dari sensor piezo-resistif dan antarmuka sensor
IC. Fungsi utama untuk mengubah tegangan output analog terkompensasi dari tekanan piezo-resistif sensor ke nilai digital.
34
Gambar 2.24 Foto chip MPL3115A2 (sensor barometer digital)
Gambar 2.25 Element peraba tekanan absolut Gambar 2.25 menunjukan bagian tekanan yang absolut pada element yang dirasakan pada chip barometer. Perbedaan tekanan positif terhadap P1 dan tekanan vakum menyebabkan diafragma menekan ke dalam. Empat buah piezoresistif pengukur ketegangan terletak pada diafragma itu yang mendeteksi defleksi diafragma sebagai stres mekanik dan memberikan tegangan keluaran. Tegangan keluaran sebanding dengan P1 dan suplai tegangan, yang akan memberikan sampel kepada ASIC dan diproses lebih lanjut, seperti penguatan sinyal dan konversi ADC. 2.5 Quaternion Quaternion ditemukan pada abad ke 19 oleh ahli matematika, William Rowan Hamilton. Quarternion adalah 4 elemen vector yang dapat digunakan untuk merotasi koordinat tiga dimensi. Secara teknis, quaternion terdiri dari satu bilangan real dan tiga bilangan kompleks.
35
𝒒 = 𝑞0 + 𝑞1 𝒊 + 𝑞2 𝒋 + 𝑞3 𝒌
(2.15)
𝑞3 ]𝑇
(2.16)
𝒒 = [𝑞0
𝑞1
𝑞2
Perkalian antara dua quaternion 𝒑, 𝒒 dilakukan dengan menggunakan perkalian Kronecker (Kronecker Product) yang ditandai dengan ⊗. Perlu diketahui bahwa dalam quaternion perkalian adalah non-komuntatif, hanyalah berupa rotasi non-komutatif. Jika rotasi satu adalah 𝒑 dan rotasi dua adalah 𝒒 maka 𝒑 ⊗ 𝒒 menunjukan rotasi. 𝑝0 𝑞0 − 𝑝1 𝑞1 − 𝑝2 𝑞2 − 𝑝3 𝑞3 𝑝0 𝑞1 + 𝑝1 𝑞0 + 𝑝2 𝑞3 − 𝑝3 𝑞2 𝒑 ⊗ 𝒒 = [𝑝 𝑞 − 𝑝 𝑞 + 𝑝 𝑞 + 𝑝 𝑞 ] 0 2 1 3 2 0 3 1 𝑝0 𝑞3 + 𝑝1 𝑞2 − 𝑝2 𝑞1 + 𝑝3 𝑞0
(2.17)
𝒑⊗𝒒 ≠ 𝒒⊗𝒑
(2.18)
Magnitude pada Quaternion : 𝑴𝒂𝒈𝒏𝒊𝒕𝒖𝒅𝒆(𝒒) = ‖𝒒‖ = √𝑞02 + 𝑞12 + 𝑞22 + 𝑞32
(2.19)
Konjugasi pada Quaternion : 𝑪𝒐𝒏𝒋(𝒒) = 𝒒∗ = [𝑞0
−𝑞1
−𝑞2
− 𝑞3 ]𝑇
(2.20)
Inverse pada Quaternion : −𝟏
𝑰𝒏𝒗(𝒒) = 𝒒
𝒒∗ = ‖𝒒‖2
(2.21)
Normalisasi pada Quaternion : 𝑵𝒐𝒓𝒎𝒂𝒍𝒊𝒛𝒆(𝒒) = [
𝑞0 ‖𝒒‖
𝑞1 ‖𝒒‖
𝑞2 ‖𝒒‖
𝑞3 𝑇 ] ‖𝒒‖
(2.22)
Rotasi vektor 3 dimensi : 0 ⃗⃗⃗ = 𝒒 ⊗ [ ] ⊗ 𝒒∗ 𝒘 ⃗ 𝒗
(2.23)
36
⃗ Rotasi pada persamaan (2.23) dapat ditulis ulang dengan mengganti 𝒗 dengan sumbu x, y dan z seperti yang ditujukan pada persamaan (2.24), (2.25) dan (2.26). 0 𝑞02 + 𝑞12 − 𝑞22 − 𝑞32 1 𝑅𝑥 (𝒒) = 𝒒 ⊗ [ ] ⊗ 𝒒∗ = [ 2(𝑞1 𝑞2 + 𝑞0 𝑞3 ) ] 0 2(𝑞1 𝑞3 − 𝑞0 𝑞2 ) 0
(2.24)
0 2(𝑞1 𝑞2 − 𝑞0 𝑞3 ) 0 ∗ 2 𝑅𝑦 (𝒒) = 𝒒 ⊗ [ ] ⊗ 𝒒 = [𝑞0 − 𝑞12 + 𝑞22 − 𝑞32 ] 1 2(𝑞2 𝑞3 + 𝑞0 𝑞1 ) 0
(2.25)
0 2(𝑞1 𝑞3 + 𝑞0 𝑞2 ) 0 ∗ 𝑅𝑧 (𝒒) = 𝒒 ⊗ [ ] ⊗ 𝒒 = [ 2(𝑞2 𝑞3 − 𝑞0 𝑞1 ) ] 0 𝑞02 − 𝑞12 − 𝑞22 + 𝑞32 1
(2.26)
Perlu diketahui bahwa hanya elemen vektor pada quaternion yang dapat diekstrak menghasilkan rotasi matriks, dimana titik rotasi pada sistem koordinat tetap seperti yang dijabarkan pada persamaan (2.28) 𝑅(𝒒) = [𝑅𝑥 (𝒒) 𝑅𝑦 (𝒒) 𝑅𝑧 (𝒒)] 𝑞02 + 𝑞12 − 𝑞22 − 𝑞32 𝑅(𝒒) = [ 2(𝑞1 𝑞2 + 𝑞0 𝑞3 ) 2(𝑞1 𝑞3 − 𝑞0 𝑞2 )
2(𝑞1 𝑞2 − 𝑞0 𝑞3 ) − 𝑞12 + 𝑞22 − 𝑞32 2(𝑞2 𝑞3 + 𝑞0 𝑞1 )
𝑞02
2(𝑞1 𝑞3 + 𝑞0 𝑞2 ) 2(𝑞2 𝑞3 − 𝑞0 𝑞1 ) ] 2 𝑞0 − 𝑞12 − 𝑞22 + 𝑞32
(2.27)
(2.28)
Rotasi dapat juga diwakili dengan menggunakan rotasi vektor pada persamaan (2.29), dimana ⃗𝒖 adalah sumbu rotasi (dalam vektor) dan 𝛼 adalah sudut rotasi. Menggunakan persaman ini dapat memiliki banyak manfaat ketika membuat kesalahan atau menentukan referensi karena memiliki koneksi fisik langsung. 𝛼 𝛼 ⃗ sin ( ) 𝒒 = cos ( ) + 𝒖 2 2
(2.29)
Untuk mewakili rotasi quaternion, konversi dari sudut Tait-Bryan ke quaternion dan dari quaternion ke sudut Euler dapat dilakukan dengan
37
memanfaatkan dua persamaan (2.30) dan (2.31). Persamaan tersebut berguna dalam kasus yang tujuannya adalah untuk mewakili sistem orientasi di sudut, sementara tetap mempertahankan dinamika keseluruhan dalam bentuk quaternion. 𝜓 𝜙 𝜓 𝜙 cos ( ⁄2) cos(𝜃⁄2) cos ( ⁄2) + sin ( ⁄2) sin(𝜃⁄2) sin ( ⁄2) 𝒒=
𝜓 𝜙 𝜓 𝜙 cos ( ⁄2) cos(𝜃⁄2) sin ( ⁄2) − sin ( ⁄2) sin(𝜃⁄2) cos ( ⁄2) 𝜓 𝜙 𝜓 𝜙 cos ( ⁄2) sin(𝜃⁄2) cos ( ⁄2) + sin ( ⁄2) cos(𝜃⁄2) sin ( ⁄2) 𝜓 𝜙 𝜓 𝜙 𝜃 𝜃 [sin ( ⁄2) cos( ⁄2) cos ( ⁄2) − cos ( ⁄2) sin( ⁄2) sin ( ⁄2)]
atan(2(𝑞1 𝑞2 + 𝑞0 𝑞3 )⁄(𝑞02 + 𝑞12 − 𝑞22 − 𝑞32 )) 𝜓 [𝜃 ] = [ asin(2(𝑞0 𝑞2 − 𝑞3 𝑞1 )) ] 2 2 2 2 𝜙 atan(2(𝑞 𝑞 + 𝑞 𝑞 )⁄(𝑞 − 𝑞 − 𝑞 + 𝑞 )) 0 1
2 3
0
1
2
(2.30)
(2.31)
3
Untuk mendapatkan offset antara 𝒒𝐴 dan 𝒒𝐵 , 𝒒𝑜𝑓𝑓𝑠𝑒𝑡 dapat dihitung dengan mengalikan 𝒒𝐴 dengan konjugasi 𝒒𝐵 seperti pada persamaan (2.32). Perkalian Kronecker dapat dimanfaatkan untuk menghasilkan offset setiap sumbu rotasi. 𝒒𝑜𝑓𝑓𝑠𝑒𝑡 = 𝒒𝐴 ⊗ 𝒒𝐵 ∗
(2.32)
2.6 Attitude Heading and Reference System Attitude Heading Reference System (AHRS) adalah sensor sistem 3 sumbu yang memberikan informasi orientasi posisi tiga dimensi (yaw, pitch roll) secara realtime. Fungsi utama maka dari AHRS adalah untuk menyediakan data orientasi. AHRS dirancang untuk menggantikan instrumen berbasis gyro tradisional dan untuk menyediakan keandalan dan akurasi yang unggul. Beberapa banyak aplikasi untuk AHRS termasuk kontrol dan stabilisasi, pengukuran dan koreksi, dan navigasi. Contoh dari kontrol dan stabilisasi bisa di mana kamera atau antena dipasang pada sistem seperti pesawat atau kapal harus
38
stabil. Pengukuran dan koreksi terbaik berlaku untuk sistem pencitraan dimana AHRS digunakan untuk memastikan arah imager tersebut menunjuk. Dan dalam navigasi, sebuah AHRS dapat digunakan untuk memberikan orientasi dan arah. AHRS terdiri dari magnetometer, sistem microelectromechanical (MEMS) accelerometer dan MEMS gyroscope pada semua tiga sumbu. Dengan kata lain, sebuah AHRS berbasis MEMS termasuk sensor untuk 3-axis magnetometer, 3axis accelerometer, dan 3-axis gyroscope. Sensor ini, dikombinasikan dengan prosesor built-in, menciptakan sistem sensor inersia yang sepenuhnya
dapat
mengukur sikap benda dalam ruang 3D. Accelerometer mengukur percepatan yang tepat - tingkat dimana kecepatan benda berubah. Mereka mengukur statis (gravitasi) atau dinamis (gerakan atau getaran) kekuatan percepatan benda yang diberikan. Accelerometer yang ideal dalam AHRS memberikan stabilitas jangka panjang, kesalahan getaran yang rendah, dan kehandalan. AHRS menuntut gyroscope yang sangat tepat, kualitas dan kinerja perangkat ini sangat berdampak pada keseluruhan sistem sensor inersia. Contoh dari gyroscope yang high-end adalah fiber-optic gyroscope, umumnya dikenal sebagai FOG. FOG memberikan informasi tingkat rotasi yang sangat tepat karena memiliki derau yang sangat kecil. Namun, FOG memiliki banyak pengembangan dan pembuatan melekat biaya serta faktor bentuk yang lebih besar dan kebutuhan daya yang lebih tinggi. Teknologi gyroscope berbasis MEMS telah menutup kesenjangan kinerja pada beberapa FOG. Ketika anjak dalam biaya dan kebutuhan daya yang lebih rendah, perangkat berbasis MEMS memberikan jawaban yang sangat baik untuk kebutuhan presisi gyroscope.
39
Magnetometer digunakan dalam AHRS untuk mengukur arah medan magnet pada suatu titik dalam ruang. Sebuah magnetometer lebih tradisional akan menjadi sistem fluxgate. Meskipun teknologi ini memberikan akurasi yang baik dan kehandalan namun tidak kondusif untuk AHRS berbasis MEMS karena ukuran bentuk yang lebih besar dan kebutuhan daya yang lebih besar. Sebuah alternatif untuk teknologi fluxgate adalah magneto-induktif (MI) teknologi penginderaan. Tidak hanya teknologi ini memberikan diinginkan faktor bentuk yang lebih kecil dan kebutuhan daya yang rendah, MI juga menyediakan resolusi yang sangat tinggi atau lebih tinggi daripada saingannya anisotropik magneto resistif (AMR) sensor dapat memberikan dengan biaya yang sama. MEMS berbasis AHRS terus dikembangkan dan ditingkatkan dalam teknologi dan aplikasinya. Sebagai persyaratan dari kedua sistem militer dan komersial yang berkembang, adanya peningkatan permintaan untuk perbaikan terus-menerus. Kedua sistem yang ada dan mereka dalam pembangunan harus memasukkan SWAP-C (Size, Weight, Power and Cost). Sederhananya, permintaan akan semakin membutuhkan sistem dan komponen mereka menjadi lebih kecil, lebih ringan, menggunakan lebih sedikit daya, dan semua dengan biaya lebih rendah. 2.6.1 Direct AHRS Fungsi trigonometri digunakan untuk menghitung sudut Yaw (𝜓), Pitch (𝜃) dan Roll (𝜙). Sudut Pitch (𝜃) dan Roll (𝜙) dapat dihitung dengan accelerometer, sedangkan sudut Yaw dapat dihitung dengan menggunakan magnetometer. 𝜓𝑑𝑖𝑟𝑒𝑐𝑡 = arctan (−
𝑚𝑎𝑔𝑦 ) 𝑚𝑎𝑔𝑥
(2.33)
40
𝜃𝑑𝑖𝑟𝑒𝑐𝑡 = arctan (−
𝑎𝑥 ) 𝑎𝑧
(2.34)
𝑎𝑦 𝜙𝑑𝑖𝑟𝑒𝑐𝑡 = arctan ( ) √𝑎𝑥 2 + 𝑎𝑧 2
(2.35)
Berdasarkan dari persamaan (2.33), (2.34) dan (2.35) maka dapat digambarkan dengan diagram blok seperti pada gambar 2.26
mag Magnetometer
Accelerometer
direct
mag y arctan mag x
direct
a arctan x az
a
direct arctan
ay ax az 2
direct
2
direct
direct
Gambar 2.26 Diagram perhitungan sudut langsung 2.6.2
Direct AHRS dengan Complementary Filter Dasar complementary filter ditujukan pada gambar 2.8.2 dimana 𝑥 dan
𝑦 adalah derau pengukuran dari sinyal 𝑧 dan 𝑧̂ adalah hasil estimasi dari filter. Dengan asumsi derau dengan frekuensi tertinggi pada 𝑦, dan derau dengan frekuensi rendah pada 𝑥. Maka 𝐺(𝑠) dapat menjadi loss-pass filter untuk derau 𝑦 𝜃𝑘 = 𝛼𝜃𝑘−1 + (1 − 𝛼)𝑎𝑘 + 𝛼𝜔𝑘 ∆𝑡
(2.36)
Dimana 𝛼=
𝜏 𝜏 + ∆𝑡
∆𝑡 = 𝑡𝑘 − 𝑡𝑘−1
(2.37) (2.38)
Pada implementasi AHRS, kombinasi perhitungan sudut langsung digabungkan dengan gyroscope sebagai input kedua.
41
direct
direct
direct
1 direct k 1 t
+ +
1 direct k 1 t
+ +
1 direct k 1 t
+ +
k
k
k
Gambar 2.27 Diagram perhitungan sudut langsung dengan Complementary Filter 2.6.3
Direct AHRS dengan Kalman Filter Pada tahun 1960, R.E Kalman mempublikasikan karya tulisnya yang
terkenal tentang solusi filter data linier secara rekursif. Sejak itu, Kalman Filter menjadi subjek penelitian dan aplikasi yang lebih luas. [14] ̂𝑘−1|𝑘−1 (previous state) merupakan perkiraan keadaan sebelumnya 𝒙 ̂𝑘|𝑘−1 (priori state) merupakan berdasarkan perkiraan dan keadaan sebelumnya. 𝒙 perkiraan keadaan pada saat sekarang berdasarkan perkiraan dan keadaan ̂𝑘|𝑘 (posteriori state) merupakan perkiraan keadaan pada saat sebelumnya. 𝒙 ̂, sekarang berdasarkan waktu k. Perkiraan suatu keadaan dinotasikan sebagai 𝒙 sedangkan keadaan dalam waktu k dinotasikan sebagai 𝒙𝑘 . Keadaan sistem pada waktu k dapat ditulis dengan persamaan (2.39) 𝒙𝑘 = 𝑭 𝒙𝑘−1 + 𝑩 𝒖𝑘 + 𝒘𝑘
(2.39)
42
Dimana 𝒙𝑘 adalah matriks keadaan, dengan estimasi posisi sudut 𝜃 dan bias 𝜃̇𝑏 , berdasarkan dari perhitungan posisi sudut dari pengukuran accelerometer atau magnetometer dan gyroscope. 𝜃 𝒙𝑘 = [ ̇ ] 𝜃𝑏
(2.40)
Matriks 𝑭 merupakan keadaan model transisi model yang berlaku pada keadaan sebelumnya 𝒙𝑘−1. Matriks 𝑩 merupakan model kontrol input. 𝑭=[
1 −∆𝑡 ] 0 1
∆𝑡 𝑩=[ ] 0
(2.41) (2.42)
Kontrol input 𝒖𝑘 pada fungsi ini diasumsikan sebagai pengukuran gyroscope (°/s) sebagai rate 𝜽𝑘 . Sedangkan 𝒘𝑘 merupakan distribusi Gaussian dimana memiliki rata-rata nol dan kovarian 𝑸 pada waktu k. 𝒘𝑘 ~𝑁(0, 𝑸𝑘 )
(2.43)
𝑸𝑘 adalah kovarian matriks derau dan pada permasalahan ini perkiraan kovarian matriks keadaaan dari accelerometer dan bias. 𝑄𝜃 𝑸𝑘 = [ 0
0 𝑄𝜃̇𝑏 ] ∆𝑡
(2.44)
Pada persamaan (2.44) kovarian matriks 𝑸𝑘 tergantung pada waktu saat ini k, maka accelerometer varian 𝑄𝜃 dan varian bias 𝑄𝜃̇𝑏 berbanding lurus dengan ∆𝑡. Pengukuran 𝑧𝑘 pada keadaan sebenarnya 𝑥𝑘 dinyatakan dengan persamaan , dimana matriks 𝑯 adalah model obeservasi yang digunakan pemetaan keadaan sebenarnya. Keadaan sebenarnya tidak dapat diobservasi karena pengukuran hanya berasal dari accelerometer atau magnetometer.
43
𝑧𝑘 = 𝑯𝑥𝑘 + 𝑣𝑘
(2.45)
𝑯 = [1 0]
(2.46)
Derau pada pengukuran terdistribusi Gaussian memiliki rata-rata nol dan kovarian 𝑹. 𝒗𝑘 ~𝑁(0, 𝑹)
(2.47)
𝑹 bukan matrik derau pengukuran namun merupakan varian dari pengukuran karena kovarian menggunakan variable yang sama dengan varian. 𝑹 = 𝐸[𝑣𝑘
𝑣𝑘 𝑇 ] = 𝑣𝑎𝑟(𝑣𝑘 )
(2.48)
Dengan asumsi pengukuran derau adalah sama dan tidak tergantung dengan waktu k. 𝑣𝑎𝑟(𝑣𝑘 ) = 𝑣𝑎𝑟(𝑣)
(2.49)
Kalman Filter bekerja secara rekursif dalam dua tahap, yaitu tahap prediksi dan tahap koreksi. Algoritma Kalman Filter untuk memprediksi posisi sudut dapat dijabarkan dalam langkah-langkah sebagai berikut : 1. Tahap Prediksi ̂𝑘|𝑘−1 = 𝑭 𝒙 ̂𝑘−1|𝑘−1 + 𝑩 𝜽̇𝑘 𝒙
(2.50)
𝑷𝑘|𝑘−1 = 𝑭 𝑷𝑘−1|𝑘−1 𝑭𝑇 + 𝑸𝑘
(2.51)
̂𝑘|𝑘−1 merupakan priori state, sedangkan 𝑷𝑘|𝑘−1 merupakan kovarian 𝒙 matriks priori error berdasarkan dari previous error kovarian matriks 𝑷𝑘−1|𝑘−1. Matriks 𝑷𝑘|𝑘−1 digunakan untuk estimasi seberapa besar hasil prediksi yang dapat dipercaya. Semakin kecil nilai 𝑷𝑘|𝑘−1 semakin akurat hasil prediksi keadaannya. Kesalahan kovarian matriks 𝑷 pada implementasinya menggunakan matriks 2x2. 𝑷=[
𝑃00 𝑃10
𝑃01 ] 𝑃11
(2.52)
44
2. Tahap Koreksi ̃𝑘 = 𝑧𝑘 − 𝑯 𝒙 ̂𝑘|𝑘−1 𝒚
(2.53)
𝑺𝑘 = 𝑯𝑷𝑘|𝑘−1 𝑯𝑇 + 𝑹
(2.54)
𝑲𝑘 = 𝑷𝑘|𝑘−1 𝑯𝑇 𝑺𝑘 −1
(2.55)
̂𝑘|𝑘 = 𝒙 ̂𝑘|𝑘−1 + 𝑲𝑘 𝒚 ̃𝑘 𝒙
(2.56)
𝑷𝑘|𝑘 = (𝑰 − 𝑲𝑘 𝑯)𝑷𝑘|𝑘−1
(2.57)
̃𝑘 adalah inovasi dari perhitungan perbedaan Pada persamaan (2.53), 𝒚 ̂𝑘|𝑘−1 . Matriks 𝑯 digunakan untuk pemetaan pengukuran 𝑧𝑘 dengan priori state 𝒙 priori state
̂𝑘|𝑘−1 ke ruang observasi pengukuran accelerometer atau 𝒙
magnetometer. ̃𝑘 = [𝒚 ̃ ]𝑘 𝒚
(2.58)
Pada persamaan (2.54), 𝑺𝑘 merupakan invonasi kovarian, dimana fungsi tersebut untuk memprediksi seberapa besar akuratnya pengukuran berdasarkan dari priori error kovarian matriks 𝑷𝑘|𝑘−1 dan kovarian matriks 𝑹. Semakin besar nilai S semakin besar pula derau yang terjadi. Pada persamaan (2.55), 𝑲𝑘 merupakan Kalman gain yang digunakan sebagai indikasi seberapa besar inovasi dapat dipercaya. Pada implementasinya Kalmain gain menggunakan matriks 2x1. 𝑲=[
𝐾0 ] 𝐾1
(2.59)
̂𝑘|𝑘 dan Persamaan (2.56) dan persamaan (2.57) menunjukan posteriori 𝒙 posteriori error kovarian matriks 𝑷𝑘|𝑘 . Dimana 𝑰 adalah matriks indentitas. 𝑰=[
1 0 ] 0 1
(2.60)
45
Berdasarkan dari persamaan-persamaan tersebut, maka tahap kalam filter dapat disederhanakan menjadi. ̂𝑘|𝑘−1 = 𝑭 𝒙 ̂𝑘−1|𝑘−1 + 𝑩 𝜽̇𝑘 𝒙 𝜃 1 −∆𝑡 𝜃 ∆𝑡 [ ̇ ] =[ ][ ̇ ] + [ ] 𝜃̇𝑘 𝜃𝑏 𝑘|𝑘−1 𝜃 0 1 0 𝑏 𝑘−1|𝑘−1 𝜃 𝜃 − ∆𝑡(𝜃̇ −̇ 𝜃𝑏 ) [ ̇ ] =[ ] 𝜃𝑏 𝑘|𝑘−1 𝜃̇
(2.61)
𝑏
Pada bahasa C dapat ditulis rate = newRate - bias; angle += dt * rate;
𝑷𝑘|𝑘−1 = 𝑭 𝑷𝑘−1|𝑘−1 𝑭𝑇 + 𝑸𝑘
[
−∆𝑡 𝑃00 ][ 𝑃10 1
𝑃01 1 ] [ 𝑃11 𝑘−1|𝑘−1 0
𝑄𝜃 −∆𝑡 𝑇 ] +[0 1
0 𝑄𝜃̇𝑏 ] ∆𝑡
𝑃 [ 00 𝑃10
𝑃01 1 ] =[ 𝑃11 𝑘|𝑘−1 0
𝑃00 𝑃10
𝑃00 − ∆𝑡(𝑃11 ∆𝑡 − 𝑃01 − 𝑃10 + 𝑄𝜃 ) 𝑃01 − 𝑃11 ∆𝑡 𝑃01 =[ ] 𝑃10 − 𝑃11 ∆𝑡 𝑃11 + 𝑄𝜃̇𝑏 ∆𝑡] 𝑃11 𝑘|𝑘−1
(2.62)
Pada bahasa C dapat ditulis P_00 P_01 P_10 P_11
+= -= -= +=
dt * (dt * P_11 - P_01 - P_10 + Q_angle); dt * P_11; dt * P_11; Q_gyro * dt;
̃𝑘 = 𝑧𝑘 − 𝑯 𝒙 ̂𝑘|𝑘−1 𝒚 ̃𝑘 = 𝑧𝑘 − [1 𝒚
𝜃 0] [ ̇ ] 𝜃𝑏 𝑘|𝑘−1
̃𝑘 = 𝑧𝑘 − 𝜃𝑘|𝑘−1 𝒚 Pada bahasa C dapat ditulis y = newAngle - angle;
(2.63)
46
𝑺𝑘 = 𝑯𝑷𝑘|𝑘−1 𝑯𝑇 + 𝑹 𝑃 𝑺𝑘 = [1 0] [ 00 𝑃10
𝑃01 [ ] 𝑃11 𝑘|𝑘−1 1
0]𝑇 + 𝑹
𝑺𝑘 = 𝑃00 𝑘|𝑘−1 + 𝑹 𝑺𝑘 = 𝑃00 𝑘|𝑘−1 + 𝑣𝑎𝑟(𝑣)
(2.64)
Pada bahasa C dapat ditulis S = P_00 + R_measure;
𝑲𝑘 = 𝑷𝑘|𝑘−1 𝑯𝑇 𝑺𝑘 −1 𝑃 𝐾 [ 0 ] = [ 00 𝐾1 𝑘 𝑃10
𝑃01 [ ]𝑇 𝑺 −1 ] 𝑃11 𝑘|𝑘−1 1 0 𝑘
𝑃 𝐾 [ 0 ] = [ 00 ] 𝑺 −1 𝐾1 𝑘 𝑃10 𝑘|𝑘−1 𝑘
𝐾 [ 0] = 𝐾1 𝑘
[
𝑃00 ] 𝑃10 𝑘|𝑘−1 𝑺𝑘
(2.65)
Pada bahasa C dapat ditulis K_0 = P_00 / S; K_1 = P_10 / S;
̂𝑘|𝑘 = 𝒙 ̂𝑘|𝑘−1 + 𝑲𝑘 𝒚 ̃𝑘 𝒙 𝜃 𝜃 𝐾 ̃ [ ̇ ] =[ ̇ ] + [ 0] 𝒚 𝐾1 𝑘 𝑘 𝜃𝑏 𝑘|𝑘 𝜃𝑏 𝑘|𝑘−1 ̃ 𝜃 𝜃 𝐾𝒚 [ ̇ ] =[ ̇ ] +[ 0 ] 𝜃𝑏 𝑘|𝑘 𝜃𝑏 𝑘|𝑘−1 ̃ 𝑘 𝐾1 𝒚 Pada bahasa C dapat ditulis angle += K_0 * y; bias += K_1 * y;
(2.66)
47
𝑷𝑘|𝑘 = (𝑰 − 𝑲𝑘 𝑯)𝑷𝑘|𝑘−1 [
𝑃00 𝑃10
𝑃01 1 ] = ([ 𝑃11 𝑘|𝑘 0
𝑃 𝐾 0 ] − [ 0 ] [1 0]) [ 00 𝐾1 𝑘 𝑃10 1
[
𝑃00 𝑃10
𝑃01 1 ] = ([ 𝑃11 𝑘|𝑘 0
𝐾 0 ]−[ 0 𝐾1 1
[
𝑃00 𝑃10
𝑃01 𝑃 ] = [ 00 𝑃11 𝑘|𝑘 𝑃10
0 𝑃00 ]) [ 0 𝑃10
𝑃01 𝐾𝑃 ] − [ 0 00 𝑃11 𝑘|𝑘−1 𝐾1 𝑃00
𝑃01 ] 𝑃11 𝑘|𝑘−1
𝑃01 ] 𝑃11 𝑘|𝑘−1 𝐾0 𝑃01 ] 𝐾1 𝑃01
(2.67)
Pada bahasa C dapat ditulis : float P00_temp = P_00; float P01_temp = P_01; P_00 P_01 P_10 P_11
2.6.4
-= -= -= -=
K_0 K_0 K_1 K_1
* * * *
P00_temp; P01_temp; P00_temp; P01_temp;
Algoritma AHRS DCM DCM (Direction Cosine Matrix) adalah rotasi matrik dimana terdiri atas
nilai kosinus sebagai penunjuk antara inisial sistem koordinat dan tujuan sistem koordinat. Algoritma DCM menggabungkan sensor gyroscope dan accelerometer untuk mendapatkan informasi orientasi quadrotor. [8] Rotasi matrik dibentuk dari kecepatan angular gyroscope. Kemudian dengan arah vector gravitasi bumi dari accelerometer, drift dari rotasi matrik hasil pembentukan gyroscope dideteksi dan menghasilkan.drift error. Drift error ini diumpan balik dengan kontrol PI (Proposional Integral) sebagai koreksi error yang terjadi. Pada gambar 2.28 menunjukan blok diagram dari proses perhitungan algoritma DCM
48
gyro
+-
t
Update Rotasi Matrik
Kontrol PI
R Euler
Normalisasi
Drift Detector
akselerometer
magnetometer
Gambar 2.28 Blok diagram algoritma DCM Pembentukan rotasi matriks dapat dibentuk dengan perubahan sudut seusai dengan persamaan (2.68). 1 𝑹𝐺 (𝑡 + 𝑑𝑡) = 𝑹𝐺 (𝑡) [ 𝑑𝜃𝑧 −𝑑𝜃𝑦
−𝑑𝜃𝑧 1 𝑑𝜃𝑥
𝑑𝜃𝑦 −𝑑𝜃𝑧 ] 1
(2.68)
Karena, 𝑑𝜃 = 𝜔 𝑑𝑡
(2.69)
Maka: 1 𝑹𝐺 (𝑡 + 𝑑𝑡) = 𝑹𝐺 (𝑡) [ 𝜔𝒛 𝑑𝑡 −𝜔𝒚 𝑑𝑡
−𝜔𝒛 𝑑𝑡 1 𝜔𝒙 𝑑𝑡
𝜔𝒚 𝑑𝑡 −𝜔𝒙 𝑑𝑡] 1
(2.70)
Dimana: 𝑹𝐺 = 𝑟𝑜𝑡𝑎𝑠𝑖 𝑚𝑎𝑡𝑟𝑖𝑘𝑠 𝜔𝒙 = 𝑘𝑒𝑐𝑒𝑝𝑎𝑡𝑎𝑛 𝑠𝑢𝑑𝑢𝑡 𝑠𝑢𝑚𝑏𝑢 𝑥 𝜔𝒚 = 𝑘𝑒𝑐𝑒𝑝𝑎𝑡𝑎𝑛 𝑠𝑢𝑑𝑢𝑡 𝑠𝑢𝑚𝑏𝑢 𝑦 𝜔𝒛 = 𝑘𝑒𝑐𝑒𝑝𝑎𝑡𝑎𝑛 𝑠𝑢𝑑𝑢𝑡 𝑠𝑢𝑚𝑏𝑢 𝑧 Dengan persamaan (2.70), rotasi matrik dapat dibentuk dengan kecepatan sudut pada gyroscope dan koreksi. Karena adanya kemungkinan numerical error pada pembacaan sensor gyroscope, menyebabkan rotasi yang dibentuk tidak
49
memenuhi orthogonalitas matrik rotasi. Sehingga perlu dilakukan koreksi orthogonalitas dan normalisasi. Matrik rotasi memiliki vektor-vektor baris yang saling tegak lurus. Seharusnya perkalian dot antar vektor baris pada rotasi bernilai 0, karena adanya numerical error pada pembacaan gyroscope menyebabkan vektor-vektor baris (juga berlaku ada kolom) dari matrik rotasi tidak saling teegak lurus dan perkalian dot antar vektor baris ini ≠ 0. 𝑟𝑦𝑥 𝑟𝑥𝑥 𝑟𝑧𝑥 𝑿 = [𝑟𝑥𝑦 ] , 𝒀 = [𝑟𝑦𝑦 ] , 𝒁 = [𝑟𝑧𝑦 ] 𝑟𝑦𝑧 𝑟𝑥𝑧 𝑟𝑧𝑧
𝒆𝒓𝒓𝒐𝒓 = 𝑿 ∙ 𝒀 = 𝑿 ∙ 𝒀 = [𝑟𝑥𝑥 𝑻
𝑟𝑥𝑦
(2.71)
𝑟𝑦𝑥 𝑟𝑥𝑧 ] [𝑟𝑦𝑦 ] 𝑟𝑦𝑧
(2.72)
Selanjutnya untuk mengatasi kondisi ini, setengah dari error dibagikan kepada kedua vektor baris: 𝑟𝑥𝑥 𝒆𝒓𝒓𝒐𝒓 𝑟 [ 𝑥𝑦 ] = 𝑿𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 = 𝑿 − 𝒀 2 𝑟𝑥𝑧 𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 𝑟𝑦𝑥 𝒆𝒓𝒓𝒐𝒓 𝑟 [ 𝑦𝑦 ] = 𝒀𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 = 𝒀 − 𝑿 2 𝑟𝑦𝑧 𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙
(2.73)
Untuk memperoleh 𝒁𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 digunakan cross product dari kedua vektor baris yang sudah orthogonal 𝒁𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 = 𝑿𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 + 𝒀𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙
(2.74)
Langkah selanjutnya adalah menjaga magnitude dari masing-masing vektor agar bernilai 1. Salah satu cara yang bisa dilakukan adalah membagi setiap vektor
baris
dengan
magnitude-nya.
Cara
ini
kurang
efektif
jika
50
diimplementasikan dalam mikrokontroler dengan keterbatasan memori dan kecepatan, karena melibatkan operasi akar dalam mencari magnitude. Cara alternatif yang dapat dipakai adalah dengan menggunakan ekspansi Taylor untuk memastikan magnitude dari setiap vektor baris agar bernilai 1. 𝑿𝑛𝑜𝑟𝑚 =
1 (3 − 𝑿𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ∙ 𝑿𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ) 𝑿𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 2
(2.75)
𝒀𝑛𝑜𝑟𝑚 =
1 (3 − 𝒀𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ∙ 𝒀𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ) 𝒀𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 2
(2.76)
𝒁𝑛𝑜𝑟𝑚 =
1 (3 − 𝒁𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ∙ 𝒁𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 ) 𝒁𝑜𝑟𝑡ℎ𝑜𝑔𝑜𝑛𝑎𝑙 2
(2.77)
Pada proses sebelumnya telah didapatkan matrik rotasi bentukan gyroscope yang orthogonal. Namun, matrik rotasi ini belum akurat untuk merepresentasikan orientasi pesawat terhadap kerangka acuan bumi, karena orientasi pesawat pada saat sistem mulai bekerja belum diketahui. Masalah kedua adalah drift dan error kuantisasi dari pembacaan gyroscope menyebabkan rotasi matrik merambat berubah tidak sesuai dengan kenyataan fisik. Untuk mengatasi masalah tersebut digunakan accelerometer sebagai referensi arah percepatan gravitasi. Jika diasumsikan tidak ada percepatan dinamis yang bekerja pada quadrotor, dari data accelerometer didapatkan vektor gravitasi yang terukur pada sistem koordinat quadrotor: 𝒂(𝑡)
𝑎𝑎𝑐𝑐𝑥 𝑎 = [ 𝑎𝑐𝑐𝑦 ] 𝑎𝑎𝑐𝑐𝑧
(2.78)
Berdasarkan rotasi matrik yang telah terbentuk, vektor gravitasi pada sistem koordinat bumi jika dilihat dari sistem koordinat quadrotor adalah:
51
𝒈𝑟𝑒𝑓 = 𝒁𝑛𝑜𝑟𝑚
(2.79)
Seharusnya vektor 𝒂(𝑡) dan 𝒈𝑟𝑒𝑓 akan sama jika rotasi matrik yang dibentuk adalah tepat. Tetapi karena adanya error yang telah disinggung sebelumnya, error pitch roll diperoleh dengan cross product dari kedua vektor: 𝒆𝒓𝒓𝒐𝒓𝑟𝑜𝑙𝑙_𝑝𝑖𝑡𝑐ℎ = 𝒂(𝑡) x 𝒈𝑟𝑒𝑓
(2.80)
Kesalahan pada sudut roll dan pitch dapat dideteksi dengan menggunakan persamaan (2.80). Selanjutnya untuk mendeteksi error yaw digunakan arah hadap dari kompas digital. Jika vektor 𝒎(𝑡) merupakan vektor arah utara magnet bumi pada sistem koordinat lokal quadrotor. 𝒎(𝑡)
𝑚𝑥 = [𝑚𝑥 ] 𝑚𝑧
𝒆𝒓𝒓𝒐𝒓𝑦𝑎𝑤 = 𝒎(𝑡) x 𝑿𝑛𝑜𝑟𝑚
(2.81)
(2.82)
Dari persamaan tersebut maka dengan pengendali PI didapatkan 𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛 dengan persamaan sebagai berikut 𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝑃 = 𝒆𝒓𝒓𝒐𝒓𝑟𝑜𝑙𝑙_𝑝𝑖𝑡𝑐ℎ x (𝐾𝑃 x |𝒂(𝑡) |)
(2.83)
𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝐼 = 𝒆𝒓𝒓𝒐𝒓𝑟𝑜𝑙𝑙_𝑝𝑖𝑡𝑐ℎ x (𝐾𝐼 x |𝒂(𝑡) |)
(2.84)
𝝎𝒄𝒐𝒓𝒓𝒆𝒄𝒕𝒊𝒐𝒏
𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝑃 + 𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝐼 = [𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝑃 + 𝜔𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛𝐼 ] 0
(2.85)
Dari persamaan (2.85), 𝝎𝒄𝒐𝒓𝒓𝒆𝒄𝒕𝒊𝒐𝒏 digunakan untuk perhitungan 𝝎(𝑡) untuk waktu cuplik berikutnya. 𝝎(𝑡) = 𝝎𝑔𝑦𝑟𝑜 (𝑡) − 𝝎𝑐𝑜𝑟𝑟𝑒𝑐𝑡𝑖𝑜𝑛 (𝑡 − 1)
(2.86)
Posisi sudut dapat diambil dengan menggunakan rotasi matrik 𝑹𝐺 , jika nilai-nilai pada rotasi matrik 𝑹𝐺 dituliskan dengan persamaan
52
𝑟𝑥𝑥 𝑟 𝑹𝐺 = [ 𝑥𝑦 𝑟𝑥𝑧
𝑟𝑦𝑥 𝑟𝑦𝑦 𝑟𝑦𝑧
𝑟𝑧𝑥 𝑟𝑧𝑦 ] 𝑟𝑧𝑧
(2.87)
Maka, 𝑟 atan ( 𝑥𝑦⁄𝑟𝑥𝑦 ) 𝜓 [ 𝜃 ] = −asin(𝑟𝑥𝑧 ) 𝑟𝑦𝑧 𝜙 [ atan ( ⁄𝑟𝑧𝑧 ) ] 2.6.5
(2.88)
Algoritma AHRS Mahony R. Mahony et all dalam penelitiannya “Nonlinear Complementary Filters
on the Special Orthogonal Group” memformulasi Explicit Complementary Filter, yaitu gabungan dari Direct Complementary Filter dan Passive Complementary Filter.
Formulasi tersebut merupakan pengukuran vectorial langsung, seperti
gravitasi atau medan magnet yang diperoleh dari IMU. Hasil dari pengamatannya tidak memerlukan rekonstruksi aljabar langsung untuk memperoleh sikap dan cocok untuk diimplementasikan pada embedded platform karena memiliki kompleksitas rendah. [4] [15]
Gambar 2.29 Diagram blok algoritma Mahony Direct Complementary Filter
53
Gambar 2.30 Diagram blok algoritma Mahony Passive Complementary Filter
Gambar 2.31 Diagram blok algoritma Mahony Explicit Complementary Filter Dalam bahasa C dapat ditulis : //=============================================================================== // MahonyAHRS.c //=============================================================================== // // Madgwick's implementation of Mayhony's AHRS algorithm. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // //================================================================================ //-------------------------------------------------------------------------------// Header files #include "MahonyAHRS.h" #include <math.h> //-------------------------------------------------------------------------------// Definitions #define sampleFreq 512.0f // sample frequency in Hz
54
#define twoKpDef (2.0f * 0.5f) #define twoKiDef (2.0f * 0.0f)
// 2 * proportional gain // 2 * integral gain
//--------------------------------------------------------------------------------// Variable definitions // 2 * proportional gain (Kp) volatile float twoKp = twoKpDef; // 2 * integral gain (Ki) volatile float twoKi = twoKiDef; // quaternion of sensor frame relative to auxiliary frame volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // integral error terms scaled by Ki volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; //---------------------------------------------------------------------------------// Function declarations float invSqrt(float x); //================================================================================== // Functions //---------------------------------------------------------------------------------// AHRS algorithm update void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float hx, hy, bx, bz; float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; float halfex, halfey, halfez; float qa, qb, qc; // Use IMU algorithm if magnetometer measurement invalid // (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az); return; } // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; // Reference direction of Earth's magnetic field hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
55
bx = sqrt(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of gravity and magnetic field halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is sum of cross product between estimated direction and measured // direction of field vectors halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * (1.0f / sampleFreq); integral error scaled by Ki integralFBy += twoKi * halfey * (1.0f / sampleFreq); integralFBz += twoKi * halfez * (1.0f / sampleFreq); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // gx gy gz
//
Apply proportional feedback += twoKp * halfex; += twoKp * halfey; += twoKp * halfez;
} // gx gy gz qa qb qc q0 q1 q2 q3
Integrate rate of change of quaternion *= (0.5f * (1.0f / sampleFreq)); *= (0.5f * (1.0f / sampleFreq)); *= (0.5f * (1.0f / sampleFreq)); = q0; = q1; = q2; += (-qb * gx - qc * gy - q3 * gz); += (qa * gx + qc * gz - q3 * gy); += (qa * gy - qb * gz + q3 * gx); += (qa * gz + qb * gy - qc * gx);
// pre-multiply common factors
// Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; } //---------------------------------------------------------------------------------// IMU algorithm update void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
56
// Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity and vector perpendicular to magnetic flux halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated and // measured direction of gravity halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { // integral error scaled by Ki integralFBx += twoKi * halfex * (1.0f / sampleFreq); integralFBy += twoKi * halfey * (1.0f / sampleFreq); integralFBz += twoKi * halfez * (1.0f / sampleFreq); gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // gx gy gz
Apply proportional feedback += twoKp * halfex; += twoKp * halfey; += twoKp * halfez;
} // gx gy gz qa qb qc q0 q1 q2 q3
Integrate rate of change of quaternion *= (0.5f * (1.0f / sampleFreq)); *= (0.5f * (1.0f / sampleFreq)); *= (0.5f * (1.0f / sampleFreq)); = q0; = q1; = q2; += (-qb * gx - qc * gy - q3 * gz); += (qa * gx + qc * gz - q3 * gy); += (qa * gy - qb * gz + q3 * gx); += (qa * gz + qb * gy - qc * gx);
// pre-multiply common factors
// Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; } //-------------------------------------------------------------------------------------// Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y;
57
} //====================================================================================== // END OF CODE //======================================================================================
2.6.6
Algoritma AHRS Madgwick Sebastian O.H. Madgwick dalam penelitiannya “Estimation of IMU and
MARG orientation using a gradient descent algorithm” menciptakan algoritma orientasi filter yang dapat dipakai untuk IMU dan MARG. IMU terdiri dari sensor accelerometer
dan
gyroscope.
Sedangkan
MARG
terdiri
dari
sensor
accelerometer, gyroscope dan magnetometer. Filter tersebut menghasilkan quaternion untuk menyajikan orientasi dalam ruang tiga dimensi. [7][16] Pada diagram 2.29 merupakan diagram blok perhitungan algoritma Madgwick, dimana Group 1 merupakan distorsi magnetik dan Group 2 merupakan kompensasi penyimpangan gyro. Filter gain 𝛽 untuk semua kesalahan pengukuran gyroscope dengan mean nol. Filter gain ζ sebagai nilai konvergen untuk menghilangkan kesalahan pengukuran gyroscope dimana nilai tersebut bukan nol.
58
Gambar 2.32 Diagram blok algoritma AHRS Madgwick Dalam bahasa C dapat ditulis : // Math library required for `sqrt' #include <math.h> // System constants #define deltat 0.001f // sampling period in seconds (shown as 1 ms) #define gyroMeasError 3.14159265358979 * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s) #define gyroMeasDrift 3.14159265358979 * (0.2f / 180.0f) // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s) #define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta #define zeta sqrt(3.0f / 4.0f) * gyroMeasDrift // compute zeta // Global system variables float a_x, a_y, a_z; // accelerometer measurements float w_x, w_y, w_z; // gyroscope measurements in rad/s float m_x, m_y, m_z; // magnetometer measurements float SEq_1 = 1, SEq_2 = 0, SEq_3 = 0, SEq_4 = 0; // estimated orientation quaternion elements with initial conditions float b_x = 1, b_z = 0; // reference direction of flux in earth frame float w_bx = 0, w_by = 0, w_bz = 0; // estimate gyroscope biases error // Function to compute one filter iteration void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z, float m_x, float m_y, float m_z) { // local system variables float norm; // vector norm float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64;
59
// float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error float w_err_x, w_err_y, w_err_z; // estimated direction of the gyroscope error (angular) float h_x, h_y, h_z; // computed flux in the earth frame // axulirary variables to avoid reapeated calcualtions float halfSEq_1 = 0.5f * SEq_1; float halfSEq_2 = 0.5f * SEq_2; float halfSEq_3 = 0.5f * SEq_3; float halfSEq_4 = 0.5f * SEq_4; float twoSEq_1 = 2.0f * SEq_1; float twoSEq_2 = 2.0f * SEq_2; float twoSEq_3 = 2.0f * SEq_3; float twoSEq_4 = 2.0f * SEq_4; float twob_x = 2.0f * b_x; float twob_z = 2.0f * b_z; float twob_xSEq_1 = 2.0f * b_x * SEq_1; float twob_xSEq_2 = 2.0f * b_x * SEq_2; float twob_xSEq_3 = 2.0f * b_x * SEq_3; float twob_xSEq_4 = 2.0f * b_x * SEq_4; float twob_zSEq_1 = 2.0f * b_z * SEq_1; float twob_zSEq_2 = 2.0f * b_z * SEq_2; float twob_zSEq_3 = 2.0f * b_z * SEq_3; float twob_zSEq_4 = 2.0f * b_z * SEq_4; float SEq_1SEq_2; float SEq_1SEq_3 = SEq_1 * SEq_3; float SEq_1SEq_4; float SEq_2SEq_3; float SEq_2SEq_4 = SEq_2 * SEq_4; float SEq_3SEq_4; float twom_x = 2.0f * m_x; float twom_y = 2.0f * m_y; float twom_z = 2.0f * m_z; // normalise the accelerometer measurement norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z); a_x /= norm; a_y /= norm; a_z /= norm; // normalise the magnetometer measurement norm = sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm; m_y /= norm; m_z /= norm; // compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x; f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y; f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z; f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 SEq_1SEq_3) - m_x; f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - m_y; f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - m_z; J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication J_12or23 = 2.0f * SEq_4; J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication J_14or21 = twoSEq_2; J_32 = 2.0f * J_14or21; // negated in matrix multiplication J_33 = 2.0f * J_11or24; // negated in matrix multiplication J_41 = twob_zSEq_3; // negated in matrix multiplication
60
J_42 J_43 J_44 J_51 J_52 J_53 J_54 J_61 J_62 J_63 J_64
= = = = = = = = = = =
twob_zSEq_4; 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in twob_xSEq_4 - twob_zSEq_2; // negated in matrix twob_xSEq_3 + twob_zSEq_1; twob_xSEq_2 + twob_zSEq_4; twob_xSEq_1 - twob_zSEq_3; // negated in matrix twob_xSEq_3; twob_xSEq_4 - 2.0f * twob_zSEq_2; twob_xSEq_1 - 2.0f * twob_zSEq_3; twob_xSEq_2;
matrix multiplication matrix multiplication multiplication
multiplication
// compute the gradient (matrix multiplication) SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; // normalise the gradient to estimate direction of the gyroscope error norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); SEqHatDot_1 = SEqHatDot_1 / norm; SEqHatDot_2 = SEqHatDot_2 / norm; SEqHatDot_3 = SEqHatDot_3 / norm; SEqHatDot_4 = SEqHatDot_4 / norm; // compute angular estimated direction of the w_err_x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3; w_err_y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2; w_err_z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
gyroscope error SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_3 + twoSEq_3 *
// compute and remove the gyroscope baises w_bx += w_err_x * deltat * zeta; w_by += w_err_y * deltat * zeta; w_bz += w_err_z * deltat * zeta; w_x -= w_bx; w_y -= w_by; w_z -= w_bz; // compute the SEqDot_omega_1 SEqDot_omega_2 SEqDot_omega_3 SEqDot_omega_4
quaternion rate measured by gyroscopes = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z; = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y; = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x; = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
// compute then integrate the estimated quaternion SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) *
rate deltat; deltat; deltat; deltat;
// normalise quaternion norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); SEq_1 /= norm;
61
SEq_2 /= norm; SEq_3 /= norm; SEq_4 /= norm; // compute flux in the earth frame SEq_1SEq_2 = SEq_1 * SEq_2; // recompute axulirary variables SEq_1SEq_3 = SEq_1 * SEq_3; SEq_1SEq_4 = SEq_1 * SEq_4; SEq_3SEq_4 = SEq_3 * SEq_4; SEq_2SEq_3 = SEq_2 * SEq_3; SEq_2SEq_4 = SEq_2 * SEq_4; h_x = twom_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 SEq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3); h_y = twom_x * (SEq_2SEq_3 + SEq_1SEq_4) + twom_y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom_z * (SEq_3SEq_4 - SEq_1SEq_2); h_z = twom_x * (SEq_2SEq_4 - SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3); // normalise the flux vector to have only components in the x and z b_x = sqrt((h_x * h_x) + (h_y * h_y)); b_z = h_z; }
2.7 Digital Motion Processor (DMP) Digital Motion Processor adalah teknologi yang diciptakan oleh perusahaan InvenSense yang ditanam pada chip sensor inersia yang bertujuan untuk mengolah data sensornya. Teknologi tersebut dapat melakukan filter data dan memproses perhitungan yang kompleks secara cepat. Bahkan teknologi ini mampu memproses data sensor dari chip lain.
Gambar 2.33 Diagram blok DMP pada MPU-9x50 dengan aplikasinya Pada gambar 2.33, chip MPU9X50 memiliki sensor gyroscope, accelerometer dan compass. Data-data pengukuran pada sensor-sensor tersebut kemudian diproses dengan DMP dan hasil perhitungannya disimpan pada buffer FIFO chip sensor. Jika buffer FIFO sudah siap dibaca oleh mikrokontroler, chip sensor memberikan sinyal interrupt.
62
Paket data DMP memiliki jumlah data sebesar 46 bytes dan memiliki frekuensi pengiriman yang dapat dikonfigurasi dengan batasan 10-200Hz.
Tabel 2.1 Struktur paket data DMP
Index Alamat W X Quaternion
2
3
4
5
6
7
4 4
9 11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
X
28
29
2
Y
30
31
2
Z
32
33
2
34
35
36
37
38
39
40
41
42
43
44
45
X Y Z
X Accelerometer
1
8
Z
Magnetometer
0
10
Y
Gyro
Jumlah (Byte)
Y Z
4 4 4 4 4
4 4 4
Pada aplikasinya, untuk dapat mengaktifasi DMP, firmware DMP harus selalu diunggah ke dalam chip sensor ketika mikrokontroler menyala pertama kalinya. Alur proses akttifasi DMP dapat dilihat pada gambar 2.34
63
START START
Initialize Initialize
Load Load DMP DMP Firmware Firmware
DMP DMP IMAGE IMAGE FIRMWARE FIRMWARE
Configure Configure DMP DMP
Reset Reset FIFO FIFO
Enable Enable DMP DMP
END END
Gambar 2.34 Alur Proses Inisialisasi DMP 2.8 Altimeter Tekanan atmosfer adalah tekanan pada titik manapun dalam atmosfer bumi. Pada umumnya, tekanan atmosfer hampir sama dengan tekanan hidrostatik yang disebabkan oleh berat udara di atas titik pengukuran. Karena udara dapat dikompresi, massa udara dipengaruhi tekanan atmosfer umum di dalam massa tersebut, yang menciptakan daerah dengan tekanan tinggi dan tekanan rendah. Pembahasan altimeter pada tugas akhir ini hanya dibatasi pada lapisan troposfer.
64
Gambar 2.35 Lapisan atmosfir bumi Standar Atmosfer Amerika Serikat 1976 memperhitungkan profil suhu pada atmosfer. Troposfer memiliki ketinggian sampai 11km dengan profil suhu linier. Hubungan antara ketinggian dan tekanan atmosfer dapat digunakan sebagai altimeter dengan tingkat keakuratan sampai beberapa sentimeter. Tekanan udara pada lapisan troposfer dapat dihitung dengan persamaan sebagai berikut:
𝑝 = 𝑝0 . (1 −
𝐿 (ℎ − ℎ0 )) 𝑇0
𝑔.𝑀 𝑅.𝐿
𝑔.𝑀
𝑅.𝐿 𝑇0 𝑝 = 𝑝0 . ( ) 𝑇0 + 𝐿(ℎ − ℎ0 )
(2.89)
Berdasarkan persamaan (2.89) maka posisi ketinggian pada lapisan troposfer dapat ditulis sebagai berikut:.
65
𝑅.𝐿
𝑇0 𝑝 𝑔.𝑀 ℎ = ℎ0 + (1 − ( ) ) 𝐿 𝑝0
(2.90)
Dimana konstanta dan variabel tersebut berdasarkan Standar Atmosfer Amerika Serikat 1976: 𝑝 = 𝑡𝑒𝑘𝑎𝑛𝑎𝑛 𝑎𝑡𝑚𝑜𝑠𝑓𝑒𝑟 (𝑃𝑎) ℎ = 𝑘𝑒𝑡𝑖𝑛𝑔𝑔𝑖𝑎𝑛 (𝑚) 𝑝0 = 𝑠𝑡𝑎𝑛𝑑𝑎𝑟 𝑡𝑒𝑘𝑎𝑛𝑎𝑛 𝑎𝑡𝑚𝑜𝑠𝑓𝑖𝑟 𝑝𝑒𝑟𝑚𝑢𝑘𝑎𝑎𝑛 𝑙𝑎𝑢𝑡 (101325𝑃𝑎) ℎ0 = 𝑘𝑒𝑡𝑖𝑛𝑔𝑔𝑖𝑎𝑛 𝑝𝑎𝑑𝑎 𝑝𝑒𝑟𝑚𝑢𝑘𝑎𝑎𝑛 𝑙𝑎𝑢𝑡 (0 𝑚) 𝐿 = 𝑇𝑖𝑛𝑔𝑘𝑎𝑡 𝑝𝑒𝑟𝑢𝑏𝑎ℎ𝑎𝑛 𝑠𝑢ℎ𝑢 (0.0065 𝐾 ⁄𝑚) 𝑇0 = 𝑆𝑡𝑎𝑛𝑑𝑎𝑟 𝑠𝑢ℎ𝑢 𝑢𝑑𝑎𝑟𝑎 𝑑𝑖 𝑝𝑒𝑟𝑚𝑢𝑘𝑎𝑎𝑛 𝑙𝑎𝑢𝑡 (288.15 𝐾) 𝑔 = 𝑝𝑒𝑟𝑐𝑒𝑝𝑎𝑡𝑎𝑛 𝑔𝑟𝑎𝑣𝑖𝑎𝑠𝑖 𝑏𝑢𝑚𝑖 (9.80665 𝑚⁄𝑠 2 ) 𝑀 = 𝑚𝑎𝑠𝑠𝑎 𝑚𝑜𝑙𝑎𝑟 𝑢𝑑𝑎𝑟𝑎 𝑘𝑒𝑟𝑖𝑛𝑔 (0.0289644 𝑘𝑔⁄𝑚𝑜𝑙 ) 𝑅 = 𝑘𝑜𝑛𝑠𝑡𝑎𝑛𝑡𝑎 𝑔𝑎𝑠 𝑢𝑛𝑖𝑣𝑒𝑟𝑠𝑎𝑙 (8.31447 𝐽⁄𝑚𝑜𝑙. 𝐾) Dengan menggunakan konstanta diatas maka persamaan (2.90) dapat disederhanakan menjadi : 0.190263 𝑝 ) ) 101325
ℎ = 44330.77 (1 − (
(2.91)
2.9 Pengendali PID Kontrol PID merupakan gabungan dari kontrol proposional, integral dan derivatif. Kontrol PID digunakan untuk menentukan presisi suatu sistem instrumentasi dengan karakteristik adanya umpan balik pada sistem tesebut. Sebuah kontroller PID menghitung nilai kesalahan sebagai perbedaan antara variable proses struktur dan set point yang diinginkan. Diagram blok kontroler PID standar disajikan pada gambar berikut ini.
66
𝑡
(2.92)
𝑑 𝑢(𝑡) = 𝐾𝑝 𝑒(𝑡) + 𝐾𝑖 ∫ 𝑒(𝑡)𝑑𝑡 + 𝐾𝑑 𝑒(𝑡) 𝑑𝑡 𝑡𝑑
K p e t
t
K i e t
Error
+ Set point -
td
Kd
+
+ +
PLANT
d e t dt
Gambar 2.36 Diagram blok pengendali PID konvensional 2.9.1
Pengendali Proposional Kontrol proporsional berfungsi untuk memperkuat sinyal kesalahan
penggerak, sehingga akan mempercepat keluaran sistem mencapai titik referensi. Hubungan antara input kontroler 𝑢(𝑡) dengan sinyal error 𝑒(𝑡) terlihat pada persamaan (2.93) 𝑢(𝑡) = 𝐾𝑝 𝑒(𝑡)
(2.93)
Dimana 𝐾𝑝 adalah konstanta proporsional. Diagram blok kontrol proporsional ditunjukan pada Gambar 2.37
+ Set point -
error
K p e t
PLANT
Gambar 2.37 Diagram blok pengendali proposional
67
2.9.2
Pengendali Integral Kontrol integral pada prinsipnya bertujuan untuk menghilangkan
kesalahan keadaan tunak (offset) yang biasanya dihasilkan oleh kontrol proporsional. Hubungan antara output kontrol integral 𝑢(𝑡) dengan sinyal error 𝑒(𝑡) terlihat pada persamaan (2.94) dan diagram bloknya pada gambar 2.38. 𝑡
𝑢(𝑡) = 𝐾𝑖 ∫ 𝑒(𝑡) 𝑑𝑡
(2.94)
𝑡𝑑
Set point
+-
error
t
K
i
e t
dt
PLANT
td
Gambar 2.38 Diagram blok pengendali integral 2.9.3
Pengendali Diferensial Kontrol derivatif dapat disebut pengendali laju, karena output kontroler
sebanding dengan laju perubahan sinyal error. Hubungan antara output derivatif 𝑢(𝑡) dengan sinyal error 𝑒(𝑡) terlihat pada Persamaan (2.95) dan blok diagram pada gambar 2.39. 𝑢(𝑡) = 𝐾𝑑
+ Set point -
error
Kd
𝑑 𝑒(𝑡) 𝑑𝑡
d e t dt
(2.95)
PLANT
Gambar 2.39 Diagram blok pengendali diferensial
68
2.9.4
Pengendali PI-D G. Szafranski dan R. Czyba dari hasil penelitiannya “Different Approaches
of PID Control UAV Type Quadrotor”, menyimpulkan bahwa pengendali PI-D merupakan pengendali yang paling praktis untuk diimplementasikan pada quadrotor [9]. Desain pengendali PI-D penelitiannya dapat dilihat pada persamaan (2.96) dan gambar 2.40. 𝑡
𝑢(𝑡) = 𝐾𝑝 𝑒(𝑡) + 𝐾𝑖 ∫ 𝑒(𝑡) 𝑑𝑡 − 𝐾𝑑 𝑡𝑑
𝑑 𝑦(𝑡) 𝑑𝑡
(2.96)
Kpet
Set point
+-
error
t
Ki et dt
+ +-
td
Kd
d y t dt
PLANT
y
Gambar 2.40 Diagram blok pengendali PI-D 2.10
Komunikasi I2C (Inter Integrated Circuit) I2C pertamakali diperkenalkan oleh Philips Semikonduktor (sekarang NXP
Semikonduktor) pada tahun 1979. Sistem Bus ini hanya memerlukan dua jalur yaitu SCL dan SDA. Sistem I2C terdiri dari saluran SCL (Serial Clock) dan SDA (Serial Data) yang membawa informasi data antara I2C dengan pengontrolnya. Piranti yang dihubungkan dengan sistem I2C Bus dapat dioperasikan sebagai
69
master dan slave. Master adalah piranti yang memulai transfer data pada I2C Bus dengan membentuk sinyal Start, mengakhiri transfer data dengan membentuk sinyal Stop, dan membangkitkan sinyal clock. Slave adalah piranti yang dialamati master. Kondisi START didefinisikan sebagai perubahan logika jalur data dari high ke low pada saat logika jalur detak berada pada logika high sedangkan kondisi STOP didefinisikan sebagai perubahan logika jalur data dari low ke high pada saat logika jalur detak berada pada logika high.
Kedua kondisi ini
diilustrasikan pada gambar
Gambar 2.41 Kondisi start dan stop pada I2C Setelah dikirimkannya kondisi start, maka dilakukan pengiriman alamat dari slave yang akan diakses. Pengiriman ini dilakukan dalam byte pertama dengan susunan dari byte pertama ini diilustrasikan pada gambar 2.42
Gambar 2.42 Format pengalamatan pada I2C
70
7-bit pertama dari byte pertama yang dikirim oleh master adalah alamat dari slave yang akan diakses. Bit terakhir akan menentukan arah data. Jika bit ini berlogika “0” maka berarti master akan mengirimkan data ke slave yang dialamati pada 7 bit pertama, sedangkan jika bit ini berlogika “1” maka master akan menerima data dari slave. Setiap piranti yang terhubung pada sistem bus I2C memiliki alamat unik tersendiri. Umumnya susunan 7-bit dari alamat piranti terdiri dari bit alamat tetap dan bit alamat yang dapat diprogram. Susunan bit alamat tetap ditentukan oleh Philips Semiconductor sebagai pemegang hak paten dari bus ini. Sedangkan susunan bit yang dapat diprogram ditentukan oleh desainer dari sistem bus yang akan digunakan. Setiap pengiriman data pada bus I2C dilakukan dalam paket satu byte. Satu bit data akan dikirim untuk setiap pulsa detak. Data harus stabil selama periode logika high dari detak. Jalur data hanya dapat berubah saat jalur detak berada pada logika low. Tranfer bit data ini diilustrasikan dengan gambar 2.43.
Gambar 2.43 Kondisi start dan stop pada I2C Dalam suatu sistem bus I2C, untuk setiap transfer data satu byte yang terjadi harus diikuti dengan suatu bit acknowledge atau bit not-acknowledge. Master akan membangkitkan pulsa detak acknowledge. Transmitter akan
71
melepaskan jalur data (SDA) selama pulsa detak acknowledge. Jika tidak terdapat kesalahan, receiver akan mem-pull down jalur SDA selama periode logika high dari pulsa detak acknowledge. Jika suatu slave receiver tidak dapat memberikan sinyal acknowledge maka slave akan membuat jalur SDA tetap berkondisi high dan master akan membangkitkan kondisi Stop untuk membatalkan pengiriman data. Jika suatu master receiver membuat jalur SDA tetap berlogika high saat pulsa detak acknowledge, master memberikan sinyal akhir dari transmisi dan slave transmitter akan “melepaskan” jalur data sehingga master dapat menggerakkan kondisi Stop. Kondisi acknowledge pada bus I2C ini diilustrasikan pada gambar 2.44.
Gambar 2.44 Kondisi acknowledge pada I2C
