Proprioceptive Visual Tracking of a Humanoid Robot Head Motion João Peixoto[1], Vitor Santos[1;2], and Filipe Silva[1;2] 1 Universidade de Aveiro, 2 Institute for Electronics Engineering and Informatics of Aveiro - IEETA {joao.peixoto,vitor,fmsilva}@ua.pt

1 The Problem 2 3 4 5 Develop a balance algorithm based on the motion of the head; Measure the motion of the head; Improve measurements accuracy by merging different sources of data. 6 7

Experimental Setup 1 2 3 4 5 6 Inertial sensors; 7 Visual sensors; Processing Unit. 6 7

Experimental Setup 1 2 3 4 5 6 7 RAZOR 9DOF - SEN 10736 Sensor A Sensor B Processing Unit Fire-wire Camera 4 5 6 7 RAZOR 9DOF - SEN 10736

Experimental Setup 1 2 3 4 5 6 7 POLOLU - MinIMU9DOF v2 Sensor A Sensor B Processing Unit Fire-wire Camera 4 5 6 7 POLOLU - MinIMU9DOF v2

Experimental Setup 1 2 3 4 5 6 7 Arduino UNO R3 Sensor A Sensor B Processing Unit Fire-wire Camera 4 5 6 7 Arduino UNO R3

Firefly MV-03MTC - Pointgrey 1 Experimental Setup 2 3 Sensor A Sensor B Processing Unit Fire-wire Camera 4 5 6 7 Firefly MV-03MTC - Pointgrey

Experimental Setup 1 2 3 4 5 6 Experiment design problem: 7 Lack of accurate ground truth. 6 7

Experimental Setup 1 2 3 4 5 6 FANUC LR Mate 200iB 7 High repeatability; High end-effector position accuracy; Reliable ground truth; Easy experiment design.

1 2 3 4 5 6 7 Fanuc 200iB POLOLU - MinIMU9DOF v2 RAZOR 9DOF - SEN 10736 Arduino UNO R3 Fire-wire Camera

Obtaining Inertial Data 1 Obtaining Inertial Data 2 3 4 5 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 6 7

Obtaining Inertial Data 1 Obtaining Inertial Data 2 3 4 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 5 6 7 𝜃 𝑧 𝑘 𝜃 𝑥 𝑘 𝜃 𝑦 𝑘

Obtaining Visual Data 1 2 3 4 5 6 Blob Detection Method; 7 Feature Extraction Method. 6 7

1 Blob Detection 2 3 4 5 6 7

1 Blob Detection 2 3 4 𝜽 𝒚 =𝟎 ° 5 6 7

1 Blob Detection 2 3 4 𝜽 𝒚 =𝟓𝟓 ° 5 6 7

Blob Detection Advantages Disadvantages 1 2 3 4 5 6 7 Direct measure of angular position; Not dependent of previous measures. Disadvantages Lack of robustness

1 Feature Extraction 2 3 4 5 6 7

1 Feature Extraction 2 3 4 5 6 7

1 Feature Extraction 2 3 4 5 6 7 𝑇

Feature Extraction 1 2 3 4 5 𝜃 𝑇 𝑘 − 𝜃 𝑇 𝑘−1 =Δ 𝜃 𝑇 𝑘 𝜃 𝑇 𝑘 − 𝜃 𝑇 𝑘−1 =Δ 𝜃 𝑇 𝑘 𝑇 = 𝑐𝑜𝑠 𝜃 𝑇 −𝑠𝑖𝑛 𝜃 𝑇 𝑑 𝑧 𝑠𝑒𝑛 𝜃 𝑇 𝑐𝑜𝑠 𝜃 𝑇 𝑑 𝑥 0 0 1 6 7 𝜃 𝑦 𝑘 = Δ𝜃 𝑇 𝑘 + 𝜃 𝑦 𝑘−1 𝜃 𝑦 𝑘 = Δ 𝜃 𝑇 𝑘 Δ 𝑡 𝑘 + 𝜃 𝑦 𝑘−1

Feature Extraction Advantages Disadvantages 1 2 3 4 5 6 7 More robust method, which can operate in several environments; Can be used for various tasks, like mapping or scene recognition. Disadvantages Relies on previous measurements

1 Visual Tracking 2 3 4 5 6 7

1 Visual Tracking 2 3 4 5 6 7

1 Results 2 3 4 5 6 7

Kalman Filter 1 2 3 4 5 6 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 6 𝑺𝒕𝒂𝒕𝒆 𝑽𝒂𝒓𝒊𝒂𝒃𝒍𝒆𝒔: 𝑥 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 7 𝒙 𝒌 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 𝑴𝒐𝒅𝒆𝒍 𝑫𝒆𝒇𝒊𝒏𝒊𝒕𝒊𝒐𝒏: 𝜃 𝑘 = 𝜃 𝑘−1 + 𝜃 𝑘−1 . ∆ 𝑡 +0,5. 𝜃 k−1 . ∆t 2 𝜃 𝑘 = 𝜃 𝑘−1 + 𝜃 k−1 .∆𝑡 𝜃 𝑘−1 = 𝑢 𝑘 = 𝟎 ( 𝜃 𝑘−1 − 𝜃 𝑘−2 ) ∆ 𝑡 𝑘 6 7 𝑥 𝑘 = 𝑨.𝑥 𝑘−1 +𝑩. 𝑢 𝑘−1 + 𝑤 𝑘 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 𝑴𝒐𝒅𝒆𝒍 𝑫𝒆𝒇𝒊𝒏𝒊𝒕𝒊𝒐𝒏: 𝜃 𝑘 = 𝜃 𝑘−1 + 𝜃 𝑘−1 . ∆ 𝑡 +0,5. 𝜃 k−1 . ∆t 2 𝜃 𝑘 = 𝜃 𝑘−1 + 𝜃 k−1 .∆𝑡 6 7 𝑥 𝑘 = 𝑨.𝑥 𝑘−1 +𝑩. 𝑢 𝑘−1 + 𝑤 𝑘 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘 𝐴= 1 0 0 Δ 𝑡 0 0 0 1 0 0 Δ 𝑡 0 0 0 1 0 0 Δ 𝑡 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 𝐵= 0,5 ∆t 2 0,5 ∆t 2 0,5 ∆t 2

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 6 𝜃 𝑥 𝑖 𝜃 𝑦 𝑖 𝜃 𝑧 𝑖 𝜃 𝑥 𝑖 𝜃 𝑦 𝑖 𝜃 𝑧 𝑖 = 1 0 0 Δ 𝑡 0 0 0 1 0 0 Δ 𝑡 0 0 0 1 0 0 Δ 𝑡 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 . 𝜃 𝑥 𝑖−1 𝜃 𝑦 𝑖−1 𝜃 𝑧 𝑖−1 𝜃 𝑥 𝑖−1 𝜃 𝑦 𝑖−1 𝜃 𝑧 𝑖−1 7 + 0,5 ∆t 2 0,5 ∆t 2 0,5 ∆t 2 𝜃 𝑥 𝑖−1 𝜃 𝑦 𝑖−1 𝜃 𝑧 𝑖−1 𝒙 𝒌 = 𝑨.𝒙 𝒌−𝟏 +𝑩. 𝒖 𝒌−𝟏 + 𝒘 𝒌 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 𝜃 𝑧 𝑘 𝜃 𝑥 𝑘 𝜃 𝑦 𝑘 5 6 𝑦 𝑘 = … 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝒚 𝒌 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 𝜃 𝑧 𝑘 𝜃 𝑥 𝑘 𝜃 𝑦 𝑘 5 𝑦 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 6 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝒚 𝒌 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 𝑦 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 … 6 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝒚 𝒌 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 𝑦 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝐶 𝑦 𝜃 𝐶 𝑦 … 6 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝒚 𝒌 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 𝑦 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝐶 𝑦 𝜃 𝐶 𝑦 … 5 𝐶= 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 1 0 … 6 𝑥 𝑘 = 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 𝜃 𝑥 𝜃 𝑦 𝜃 𝑧 7 𝑥 𝑘 = 𝐴.𝑥 𝑘−1 +𝐵. 𝑢 𝑘−1 + 𝑤 𝑘 𝑦 𝑘 =𝑪. 𝑥 𝑘 + 𝑣 𝑘

Data Merging Using Kalman Filter 1 Data Merging Using Kalman Filter 2 3 4 5 I V MD Kalman Filter 6 *1 7 *1 𝐺 𝑘 = ……………… ……………… ……………… ……………… ……………… ……………… 0 0 0 0 0 0 0 0 0 0 0 0

1 Results 2 3 4 5 6 7

1 Results 2 3 4 5 6 7

Results 𝑢 𝑘 = ( 𝜃 𝑘−1 − 𝜃 𝑘−2 ) ∆ 𝑡 𝑘 1 2 3 4 5 6 Kalman Filter Inertial Data Visual Data Merged Data Ground Truth No Treatment Kalman Filter 𝑢 𝑘 =0 𝑢 𝑘 = ( 𝜃 𝑘−1 − 𝜃 𝑘−2 ) ∆ 𝑡 𝑘 6 7

Experiment 1

Experiment 1 (Error)

Experiment 2

Experiment 2 (Error)

Conclusions FANUC 200iB provides accurate and reliable ground truth; Merging inertial and visual data will wield better results than the original data by itself; Kalman Filter is robust to noise; Worst cases will have better improvement; Extensible tool/approach.