KATA PENGANTAR Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN 2088-6985 Vol. 02, No 1, pp 23-30, 2011 p-ISSN 2087-3379 © 2011 RCEPM - LIPI All rights reserved doi: 10.14203/j.mev.2011.v2.23-30 PENGGUNAAN EXTENDED KALMAN FILTER SEBAGAI ESTIMATOR SIKAP PADA SISTEM KENDALI SERVO VISUAL ROBOT Noor Cholis Basjaruddin Jurusan Teknik Elektro - Politeknik Negeri Bandung Jl. Gegerkalong Hilir, Ds. Ciwaruga, Kotak Pos 1234, Bandung Jawa Barat 40012, Indonesia ppmteam@gmail.com Diterima: 28 Maret 2011; Direvisi: 11 April 2011; Disetujui: 23 April 2011; Terbit online: 7 Juli 2011. Abstrak Extended Kalman Filter (EKF) merupakan versi nonlinear dari tapis Kalman dan merupakan tapis yang biasa dipakai pada estimasi peubah keadaan nonlinear. Pada penelitian ini EKF diterapkan untuk mengolah ciri citra dari kamera tunggal yang terpasang pada end effector sebuah robot. Data yang dihasilkan oleh EKF kemudian diolah untuk mendapatkan parameter gerak. Simulasi sistem kendali servo visual dibangun dengan tujuan untuk meneliti penggunaanEKF sebagai estimator sikap. Hasil simulasi menggunakan program Matlab menunjukkan bahwa EKF mampu mengestimasi sikap robot dengan baik. Kata kunci: kendali servo visual, position-based visual servo, tapis kalman, estimator sikap. Abstract Extended Kalman Filter (EKF) is the non-linear version of Kalman filter and the said filter is usually used in nonlinear state estimation. In this study EKF is applied to process the image features of a single camera mounted on the end effector of a robot. Data generated by the EKF then is to be processed to obtain the motion parameters. Simulation of visual servo control system was built with the aim to examine the use of the EKF as a pose estimator. The simulation results using Matlab show that the EKF is able to well estimate the robot pose. Keywords: visual servo control system, position-based visual servo, extended kalman filter, pose estimation. I. PENDAHULUAN Estimasi peubah keadaan nonlinier menjadi bagian dari teori estimasi yang terus dikembangkan. Berbagai metode estimasi peubah keadaan nonlinier yang saat ini dikembangkan antara lain EKF, Unscented Kalman filter (UKF), dan Particle filters [1]. Sebagai estimator, EKF bisa digunakan pada berbagai sistem nonlinier seperti pada sistem kendali servo visual [2]. Sistem kendali servo visual adalah sistem kendali yang memanfaatkan kamera sebagai sensor posisi. Sistem kendali ini lazim dipakai pada robot canggih yang menggunakan kamera sebagai mata [3] [4]. II. METODE PENELITIAN A. Sistem Kendali Servo Visual Robot Sistem kendali servo visual robotdengan kamera terpasang pada end effector dapat diterangkan dengan memperhatikan Gambar 1. W𝑂𝑂 𝐵𝐵 dan W𝐸𝐸 𝐵𝐵 menyatakan posisi dan orientasi dari objek danend effector robot dalam kerangka base (dasar) robot. W𝐸𝐸 𝑂𝑂 dan W𝐸𝐸𝑟𝑟𝑚𝑚𝑟𝑟𝑟𝑟 𝑂𝑂 menyatakan posisi dan orientasi aktual dan yang diinginkan dari end effector robot dalam kerangka objek. ∆W𝐸𝐸 𝐸𝐸 adalah vektor galat dalam kerangka end effector. Tujuan pengendalian robot adalah mengendalikan gerakan robot agar vektor galat menjadi minimum untuk nilai acuan W𝐸𝐸𝑟𝑟𝑚𝑚𝑟𝑟𝑟𝑟 𝑂𝑂 yang berubah. Base Object B OW B EW O EW OEreffW E EW∆ Gambar 1. Hubungan vektor pada sistem. http://dx.doi.org/10.14203/j.mev.2011.v2.23-30 Penggunaan Extended Kalman Filter Sebagai Estimator Sikap pada Sistem Kendali Servo Visual Robot (Noor Cholis Basjaruddin) JMEV 02 (2011) 23-30 24 Gambar 2. Blok diagram sistem. Pada Gambar 2 dapat dilihat blok diagram sistem. OEŴ adalah estimasi posisi dan orientasi end effector dalam kerangka objek yang diperoleh dari EKF. Masukan EKF adalah vektor ciri f yaitu sebuah vektor satu dimensi yang mengandung informasi ciri. Ciri citra dihasilkan dari ekstraksi citra yang ditangkap oleh kamera. B. Estimasi Sikap 3D Menggunakan Extended Kalman Filter Langkah pertama dalam estimasi sikap relatif adalah membuat hubungan geometrik antara titik ciri yang terdefinisi di dalam kerangka objek dan hubungannya dengan titik-titik bidang citra di dalam kerangka kamera. Lihat Gambar 3. Gambar 3. Proyeksi citra ciri objek. Keterangan Gambar 3. Oo - XoYoZo Kerangka acuan objek Oc - XcYcZc Kerangka acuan kamera dengan sumbu Zc berimpit dengan sumbu optik Oi – XiYi Kerangka acuan citra To j o j o j o j )Z,Y,(XP =  Vektor koordinat dari titik ciri objek ke j di dalam kerangka objek Tc j c j c j c j )Z,Y,(XP =  Vektor koordinat dari titik ciri objek ke j di dalam kerangka kamera i j i j y,x Proyeksi koordinat titik ciri ke-j di dalam kerangka citra Karakteristik intrinsik kamera: F Panjang fokus kamera Px, Py Jarak didalam pixel sepanjang sumbu xi dan yi (m/pixel) Transformasi koordinat-koordinat titik ciri ke- j di dalam kerangka objek yang berhubungan dengan koordinat di dalam kerangka kamera adalah: o j c j Pψ)α,,R(TP φ+= (1) o jP dianggap diketahui dari model CAD, [ ]TZY,X,T =  dan R(ϕ,α,ψ) adalah matrik rotasi. R(ϕ,α,ψ)=           − −+ +− ααα φψαφφψαφαφ φψαφφψαφαφ CC SC S CC CSS CC SSS CS SS SSC CSSSC CC ψψ ψψ ψψ (2) Dari Gambar 3 dapat diturunkan persamaan lokasi titik ciri proyeksi di dalam kerangka citra, yaitu: c jy c ji jc jx c ji j ZP FY y , ZP FX x −=−= (3) Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN 2088-6985 Vol. 02, No 1, pp 23-30, 2011 p-ISSN 2087-3379 25 Gabungan persamaan 1 dan 2 menghasilkan persamaan yang mendefinisikan lokasi ciri citra sebagai fungsi nonlinier dari parameter- parameter sikap. C. Algoritma Tapis Kalman Tapis Kalman mendefinisikan estimasi optimal dari state (keadaan) dengan dinamika yang terspesifikasi melalui sebuah model dinamika ruang keadaan linear dan pengukuran keluaran yang terdefinisikan sebagai fungsi linear dari keadaan. Hal ini menganggap bahwa rata- rata derau Gaussian adalah nol pada masukan model dinamika dan pada kesalahan pengukuran keluaran. Model dinamika menggambarkan gerakan objek relatif terhadap kamera dalam bentuk fungsi keadaan sistem yang didalamnya terkandung parameter sikap. Jika objek diam yang diperhatikan, model dinamika ini hanya berhubungan dengan karakteristik dinamika robot. Jika objek bergerak, gerakan relatif tidak akan terdefinisi. Hal ini disebabkan perubahan gerakan relatif tidak dapat dinyatakan oleh sebuah model dinamika yang terstruktur, sehingga model dinamika pendekatan dibutuhkan. Model yang dibangun didasarkan pada anggapan bahwa gerakan relatif dalam bidang 3D adalah relatif lambat terhadap laju cuplik dan cukup halus sehingga turunan pertama parameter sikap )ψαφ  ,,,Z,Y,X ( dapat dianggap tetap sepanjang periode cuplik. Model dinamika sistem waktu diskrit seperti persamaan berikut: wk = Awk-1 + γk (4) yang mana, w = [ ]T,,,,,,ZZ,,YY,,XX, ψψααφφ  γ = vektor derau gangguan, dianggap rata- ratanya nol dan derau Gaussian dengan kovarian Q T = periode cuplik K = waktu cuplik 1212 1 0 T 1 . . . 1 0 T 1 1 0 T 1 A x                                 = (5) Keluaran model diberibatasan sebagai hubungan geometri antara pengukuran vision zk dari lokasi- lokasi titik citra dan vektor keadaan sistem wk. Untuk model dinamika (persamaan 4), paling sedikit 6 persamaan pengukuran bebas yang dibentuk dengan menggunakan 3 ciri, dibutuhkan sehingga seluruh keadaan sistem teramati. Perhatikan (𝑋𝑋𝑗𝑗 0 , 𝑌𝑌𝑗𝑗 0 , 𝑍𝑍𝑗𝑗 0 ) , j = 1,2,3 adalah koordinat tiga citra dan �𝑋𝑋𝑗𝑗 𝑤𝑤, 𝑌𝑌𝑗𝑗 𝑤𝑤� 𝑘𝑘 adalah pengukuran ke-k lokasi ciri citra yang berhubungan. Model keluaran tapis Kalman dapat diturunkan sebagai, zk = G(wk) + vk (6) yang mana, [ ]Tk,,,,, i3i3i2i2i1i1k yxyxyx z = (7) T k ,,,,,         = c 3y c 3 c 3x c 3 c 2y c 2 c 2x c 2 c 1y c 1 c 1x c 1 k ZP Y ZP X ZP Y ZP X ZP Y ZP X F- )w(G (8) vk adalah derau pengukuran yang dianggap termasuk dalam derau Gaussian rata-rata nol dengan kovarian R. Disebabkan keluaran model nonlinier, maka dibutuhkan tapis Kalman terluaskan (Extended Kalman Filter). EKF berfungsi sebagaimana sebuah sensor posisi dinamis yang menerima pengukuran citra kasar sebagai masukan dan menyiapkan parameter sikap terestimasi pada setiap terjadi pencuplikan. Persamaan rekursif untuk EKF diberikan sebagai berikut: Persamaan prediksi, 1-k1,-k1-kk, ˆˆ wAw = (9) 1-k T 1-k1,-k1-kk, QAAPP += (10) Penguatan Kalman -1T k-1kk,k T k-1kk, )CPCR(CPK k+= (11) Pembaharuan estimasi, ))ˆ((ˆˆ k -1kk,-1kk,kk, wGzKww −+= (12) -1kk,-1kk,kk, PKCP P k−= (13) Penggunaan Extended Kalman Filter Sebagai Estimator Sikap pada Sistem Kendali Servo Visual Robot (Noor Cholis Basjaruddin) JMEV 02 (2011) 23-30 26 D. Matrik Q dan R Matrik Q adalah matrik kovarian yang berhubungan dengan derau gangguan dinamik pada model sistem, sedangkan matrik R adalah matrik kovarian yang berhubungan dengan derau pengukuran pada model keluaran sistem. Derau gangguan dinamik pada model sistem menggambarkan pengaruh dari pengabaian bentuk turunan orde tinggi dari tiap parameter sikap. Atas dasar itu maka pembobotan dalam matrik Q hendaknya dipilih lebih besar untuk perubahan yang lebih cepat pada parameter gerak dan dipilih pembobotan lebih kecil untuk perubahan lebih lambat dari parameter gerak. Jika dianggap derau gangguan pada model sistem hanya terjadi pada parameter keadaan kecepatan )ψαφ  ,,,Z,Y,X ( , maka pembobotan matrik Q yang berhubungan dengan parameter keadaan posisi )ψαφ ,,Z,Y, X,( dapat dipilih nol. Pembobotan matrik Q yang berhubungan dengan kecepatan dapat diperoleh melalui perhitungan variansi galat kecepatan dari beberapa data. Matrik kovarian R dapat ditentukan berdasarkan variansi galat pengukuran ciri citra ),i yx( i dari beberapa data. Derau pada model sistem maupun derau pada pengukuran keduanya dianggap tidak berkorelasi, merupakan derau putih dan memiliki fungsi kepadatan probabilitas Gaussian dengan rata-rata nol. Anggapan bahwa derau tidak berkorelasi memberi kemudahan dalam pemilihan bentuk matrik Q dan R. Bentuk matrik yang sesuai dengan anggapan tersebut adalah matrik diagonal dan elemen-elemen diagonalnya merupakan variansi galat proses untuk Q serta variansi galat pengukuran untuk R. Anggapan bahwa rata-rata dari fungsi kepadatan Gaussian adalah nol memudahkan dalam perhitungan variansi galat. Variansi galat dengan anggapan tersebut adalah sama dengan nilai rata-rata kuadratnya. E. Diagram Alir Simulasi Gambar 4 menunjukkan diagram alir simulasi penggunaan EKF sebagai estimator sikap robot. Program simulasi diawali dengan penentuan sikap end effector terhadap objek yang dikehendaki (WEORef), posisi objek terhadap base robot (WOB), Jumlah iterasi EKF (k), lama waktu simulasi (t) dan sikap robot pada saat awal simulasi. Masukan yang diberikan pada program adalah matrik bobot Q dan R. Saat awal simulasi perhitungan kinematika balik didasarkan pada selisih WEORef dan sikap robot pada saat awal simulasi. Nilai sudut sendi robot kemudian digunakan untuk perhitungan persamaan dinamika dan kinematika. Hasil perhitungan tahap ini adalah sikap robot terhadap base (WEB). Sikap robot kemudian dibandingkan dengan posisi objek terhadap base (WOB) dan hasilnya adalah WEO yaitu posisi endeffector terhadap objek. Posisi objek bisa dikenali oleh kamera yang ada di endeffector. Ciri yang dihasilkan oleh kamera kemudian diestimasi oleh EKF secara iteratif menggunakan persamaan (9)-(13). Hasil estimasi posisi endeffector adalah WEOekf. Hasil estimasi ini kemudian dibandingkan dengan WEOref dan diperoleh selisih sikap yang terjadi dengan sikap yang dikehendaki (∆WEE). Gambar 4. Diagram alir simulasi. Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN 2088-6985 Vol. 02, No 1, pp 23-30, 2011 p-ISSN 2087-3379 27 Hasil estimasi kadang terlalu jauh dari yang dikehendaki, hal ini mengakibatkan robot tidak bisa mencapai posisi tersebut. Secara matematis kondisi ini ditandai dengan tidak terpecahkannya persamaan kinematika (divergen). Pemilihan matrik Q dan R yang tepat bisa mengatasi masalah ini [5]. III. HASIL DAN PEMBAHASAN Pada penelitian ini dibangun program simulasi untuk mengamati kerja EKF dalam mengestimasi sikap robot. Simulasi dibangun menggunakan program Matlab dengan Robotic Toolbox. Pemilihan matrik Q dan R menjadi fokus dalam simulasi ini. Simulasi bagian pertama bertujuan untuk meneliti pengaruh pemilihan matrik Q. Pada simulasi ini dipilih matrik R tertentu dengan matrik Q berubah-ubah. Simulasi bagian kedua bertujuan untuk meneliti pengaruh pemilihan matrik R. Pada simulasi ini dilakukan sebaliknya, matrik Q dipilih tetap dan matrik R berubah-ubah. A. Pengaruh Pemilihan Matrik Q Pada simulasi ini matrik R dianggap tetap yaitu matrik diagonal dengan elemen diagonal 1000. Hasil simulasi yang ditampilkan dalam gambar hanya untuk posisi sikap robot. Hasil simulasi untuk orientasi sikap robot ditunjukkan dalam bentuk tabel. Simulasi 1;pada simulasi ini, R dan Q adalah matriks diagonal dimana R berelemen 1.000 dan Q berelemen 0,01. Gambar 5 menunjukkan estimasi posisi sikap robot pada simulasi 1. Simulasi 2; pada simulasi ini, R dan Q adalah matriks diagonal dimana R berelemen 1.000 dan Q berelemen 0 untuk pembobotan pada parameter sikap serta 1.000 untuk pembobotan Tabel 1. Perbandingan rata-rata galat estimasi antara simulasi 1, 2 dan 3. Arah Rata-rata Galat Estimasi Simulasi 1 Simulasi 2 Simulasi 3 X -0,0064 0,0024 -0,0037 Y 0,0016 0,0005 -0,0004 Z -0,0008 -0,0024 -0,0000 r 0,0034 0,0038 0,0301 p -0,0011 0,0115 -0,0230 y -0,0040 0,0000 0,0049 pada laju sikap. Gambar 6 menunjukkan estimasi posisi sikap robot pada simulasi 2. Simulasi 3; pada simulasi ini, R dan Q adalah matriks diagonal dimana R berelemen 1.000 dan Q berelemen 0,1 untuk pembobotan pada parameter sikap serta 0 untuk pembobotan pada laju sikap. Gambar 7 menunjukkan estimasi posisi sikap robot pada simulasi 3. Berdasarkan simulasi 1 dan 2 dapat disimpulkan bahwa pemilihan matrik Q berbentuk diagonal dengan elemen matrik dipilih nol untuk yang berhubungan dengan parameter posisi dan dipilih 1000 untuk yang berhubungan dengan parameter kecepatan menghasilkan kinerja EKF yang lebih baik untuk estimasi arah X dan Y. Hal ini mendukung anggapan awal bahwa derau gangguan dinamis terjadi pada parameter keadaan kecepatan ),,,Z,Y,X ( ψαφ  . Kesimpulan yang sama juga didapat dari hasil simulasi 2 yang menunjukkan kinerja EKF yang lebih baik dibanding simulasi 3. Perbandingan rata-rata galat estimasi pada simulasi 1, 2, dan 3 ditampilkan dalam Tabel 1. B. Pengaruh Pemilihan Matrik R Pada simulasi ini matrik Q dianggap tetap yaitu matrik diagonal dengan elemen 0.01. R adalah matrik diagonal dengan elemen 100.000. Hasil simulasi estimasi posisi sikap robot pada simulasi ini dapat dilihat pada Gambar 8. Rata-rata galat estimasi pada simulasi ini dibandingkan dengan hasil simulasi 1 pada simulasi pengaruh pemilihan matrik Q (III.A) dan ditampilkan dalam Tabel 2. Hasil simulasi ini menunjukkan bahwa pembesaran nilai elemen matrik R akan menjadikan EKF lebih tepat dalam mengestimasi sikap robot pada arah X dan Y. Tabel 2. Perbandingan rata-rata galat estimasi antara simulasi pemilihan matrik R dan pemilihan matrik Q. Arah Rata-rata Galat Estimasi R=1000 R=100.000 X -0,0064 0,0024 Y 0,0016 0,0005 Z -0,0008 -0,0024 r 0,0034 0,0032 p -0,0011 0,0115 y -0,0040 0,0000 Penggunaan Extended Kalman Filter Sebagai Estimator Sikap pada Sistem Kendali Servo Visual Robot (Noor Cholis Basjaruddin) JMEV 02 (2011) 23-30 28 Gambar 5. Estimasi posisi sikap robot pada simulasi 1. Gambar 6. Estimasi posisi sikap robot pada simulasi 2. Gambar 7. Estimasi posisi sikap robot pada simulasi 3. Journal of Mechatronics, Electrical Power, and Vehicular Technology e-ISSN 2088-6985 Vol. 02, No 1, pp 23-30, 2011 p-ISSN 2087-3379 29 Gambar 8. Estimasi posisi sikap robot dengan R berelemen 100.000 dan Q berelemen 0,01. IV. KESIMPULAN Extended Kalman Filter (EKF) dapat diterapkan sebagai estimator sikap robot pada sistem kendali servo visual. Keberhasilan kerja EKF sebagai estimator sangat dipengaruhi oleh pemilihan matrik Q dan R. Pengamatan pengaruh matrik Q bisa dilakukan dengan cara memilih satu matrik R dan mengubah matrik Q hingga dicapai estimasi yang terbaik. Selanjutnya pilih matrik Q yang menghasilkan estimasi terbaik dan ubah matrik R sehingga dicapai estimasi yang lebih baik. Berdasarkan penelitian elemen matrik Q yang jauh lebih kecil dari elemen matrik R akan menghasilkan kerja EKF yang lebih baik. Pada saat pelaksanaan simulasi perlu diperhatikan pemilihan matrik Q dan R agar didapat konvergensi perhitungan kinematika tak langsung. Penelitian ini dapat dikembangkan lebih lanjut dengan menggunakan metoda estimasi lain seperti Adaptive Kalman Filter (AKF), Unscented Kalman Filter (UKF) atau Particle Filter (PF). DAFTAR PUSTAKA [1]. John L. Crassidis, “Survey of Nonlinear Attitude Estimation Methods”, Journal Of Guidance, Control, And Dynamics, Vol. 30, No. 1, January–February 2007 [2]. William J Wilson, “Visual Servo Control of Robots Using Kalman Filter Estimates of Robot Pose Relative to Work-Pieces”, Proceedings IFAC 12th World Congress, page 9-399 to 9-404, Sydney, 1993 [3]. PI. Corke, “Visual Control of Robot Manipulators – A Review”, Visual Servoing Real-Time Control of Robot Manipulators Based on Visual Sensory Feedback, World Scientific Publishing,1993 [4]. PI.Corke, “Visual Control of Robots, High-Performance Visual Servoing”, John Wiley & Sons Inc, New York, 1996 [5]. Noor Cholis Basjaruddin, “Simulasi Kendali Servo Visual Dengan Tapis Kalman Sebagai Estimator Sikap Pada Robot PUMA 560”, Tesis, Program Pascasarjana ITB, Bandung, 2002 [6]. J.Hill and WT Park, “Real Time Control of a Robot with a Mobile Camera”, 9th, International Symposium on Industrial Robots, pp-233-246, March 1979. [7]. KS.Fu et.al, “Robotics, Control, Sensing, Vision and Intelligence”, McGrawHill International Editions,Singapura, 1987 [8]. Luh, J.Y.S et.al, “On-Line Computational Scheme for Mechanical Manipulators”, Trans. ASME, J.Dynamic Systems, Measurements and Control, vol.120, pp.69-76 [9]. J.Wang, WJ.Wilson, “3D Relative Position and Orientation Estimation Using Kalman Filtering for Robust Control”, Proceedings for the 1992 IEEE Robotics and Automation Conference, Nice, France, May 10-15, 1992 [10]. PI. Corke, “Robotic Toolbox (release 5)”, Pinjarra Hills, Australia, 1999 [11]. RD Klafter, TA. Chmielewski, and M.Negin, “Robotic Engineering An Integrated Approach”, Prentice Hall, USA, 1989 [12]. Robert Grover Brown & Patrick YC Hwang, “Introduction to Random Signals Penggunaan Extended Kalman Filter Sebagai Estimator Sikap pada Sistem Kendali Servo Visual Robot (Noor Cholis Basjaruddin) JMEV 02 (2011) 23-30 30 and Applied Kalman Filtering”, John Wiley & Sons Inc, Canada, 1997 [13]. Koichi Hashimoto, “Visual Servoing Real Time Control of Robot Manipulator Based on Visual Sensory Feedback”, World Scientific Publishing, Singapura, 1993 [14]. Hutchinson, Seth, “A Tutorial on Visual Servo Control”, IEEE Transactions on Robotics and Automation, Vol.12 No.5, 1996 [15]. J. Rajruangrabin, et.al, “Simultaneous Visual Tracking and Pose Estimation with Applications to Robotic Actors”,The 2008 International Conference on Image Processing, Computer Vision, and Pattern Recognition, USA, 2008 [16]. Jetto, Leopoldo, “Development and Experimental Validation of an Adaptive Extended Kalman Filter for the Localization of Mobile Robots”, IEEE Transactions On Robotics And Automation, Vol. 15, No. 2, USA,1999 [17]. Arko Djajadi, et.al, “A Model Vision of Sorting System Application Using Robotic Manipulator”, TELKOMNIKA, Vol.8, No.2 Yogyakarta, 2010