## P ENGGUNAAN E XTENDED K ALMAN F ILTER S EBAGAI E STIMATOR S IKAP PADA S ISTEM K ENDALI S ERVO V ISUAL R OBOT

## 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. P ENDAHULUAN
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. M ETODE P ENELITIAN

## 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 dan end 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 O W B E W O E W O Ereff W E E W ∆
Gambar 1. Hubungan vektor pada sistem.

## Gambar 2. Blok diagram sistem.
Pada  Gambar  2 dapat dilihat blok diagram
sistem. O E W ˆ 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.
O o - X o Y o Z o Kerangka acuan objek O c - X c Y c Z c Kerangka acuan kamera dengan sumbu Z c berimpit dengan sumbu optik O i – X i Y i Kerangka acuan citra
T o j o j o j o j ) Z , Y , (X P =

Vektor koordinat dari titik ciri objek ke  j  di dalam kerangka objek
T c j c j c j c j ) Z , Y , (X P =  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 x i dan y i
(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( T P φ + = (1)
o j P dianggap diketahui dari model CAD,
[ ] T Z Y, X, T =  dan R( ϕ ,α,ψ) adalah matrik rotasi.
R( ϕ ,α,ψ)=
    
  
  − − + + − α α α φ ψ α φ φ ψ α φ α φ φ ψ α φ φ ψ α φ α φ C C S C S C C C S S C C S S S C S S S S S C C S S S C C C ψ ψ ψ ψ ψ ψ (2)
Dari  Gambar  3 dapat diturunkan persamaan lokasi titik ciri proyeksi di dalam kerangka citra, yaitu:
c j y c j i j c j x c j i j Z P FY y , Z P FX x − = − = (3)
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:
w k = Aw k-1 + γ k (4)
yang mana,
w = [ ] T , , , , , , Z Z, , Y Y, , X X, ψ ψ α α φ φ       γ = vektor derau gangguan, dianggap rata- ratanya nol dan derau Gaussian dengan kovarian Q
T = periode cuplik
K = waktu cuplik 12 12 1 0 T 1 . . . 1 0 T 1 1 0 T 1 A x    
          

## 

##             



= (5)
Keluaran model diberibatasan sebagai hubungan geometri antara pengukuran vision z k dari lokasi- lokasi titik citra dan vektor keadaan sistem w k . 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,
z k = G ( w k ) + v k (6)
yang mana,
[ ] T k , , , , , i 3 i 3 i 2 i 2 i 1 i 1 k y x y x y x z = (7)
T k , , , , ,         = c 3 y c 3 c 3 x c 3 c 2 y c 2 c 2 x c 2 c 1 y c 1 c 1 x c 1 k Z P Y Z P X Z P Y Z P X Z P Y Z P X F - ) w ( G (8)
v k 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 - k 1, - k 1 - k k, ˆ ˆ w A w = (9)
1 - k T 1 - k 1, - k 1 - k k, Q A AP P + = (10)
Penguatan Kalman
-1 T k -1 k k, k T k -1 k k, ) C P C R ( C P K k + = (11)
Pembaharuan estimasi,
)) ˆ ( ( ˆ ˆ k -1 k k, -1 k k, k k, w G z K w w − + = (12)
-1 k k, -1 k k, k k, P KC P P k − = (13)

## 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 y x ( 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).
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. H ASIL DAN P EMBAHASAN
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
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.
Gambar 8. Estimasi posisi sikap robot dengan R berelemen 100.000 dan Q berelemen 0,01.

## IV. K ESIMPULAN
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).

## D AFTAR P USTAKA
[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
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