1. 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

2. 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

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

4. 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

5. 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

6. 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

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

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

9. 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.

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

11. Obtaining Inertial Data1
Obtaining Inertial Data 2 3 4 5
𝜃 𝑧
𝜃 𝑥
𝜃 𝑦 6 7

12. Obtaining Inertial Data1
Obtaining Inertial Data 2 3 4
𝜃 𝑥
𝜃 𝑦
𝜃 𝑧 5 6 7
𝜃 𝑧 𝑘
𝜃 𝑥 𝑘
𝜃 𝑦 𝑘

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

14. 1
Blob Detection 2 3 4 5 6 7

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

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

17. 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

18. 1
Feature Extraction 2 3 4 5 6 7

19. 1
Feature Extraction 2 3 4 5 6 7

20. 1
Feature Extraction 2 3 4 5 6 7 𝑇

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

22. 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

23. 1
Visual Tracking 2 3 4 5 6 7

24. 1
Visual Tracking 2 3 4 5 6 7

25. 1
Results 2 3 4 5 6 7

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

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

28. Data Merging Using Kalman Filter1
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 + 𝑤 𝑘 𝑦 𝑘 =𝐶. 𝑥 𝑘 + 𝑣 𝑘

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

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

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

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

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

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

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

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

37. 1
Results 2 3 4 5 6 7

38. 1
Results 2 3 4 5 6 7

39. 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

40. Experiment 1

41. Experiment 1 (Error)

42. Experiment 2

43. Experiment 2 (Error)

44. 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.