Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR...
Transcript of Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR...
![Page 1: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/1.jpg)
Integração de Dados Visuais e Inerciais Para o
Equilíbrio de Um Robô HumanóideJoão Carlos Pimentel Fidalgo Peixoto
60140
Universidade de Aveiro,Departamento de Engenharia Mecânica
Orientador: Professor Vítor SantosCoorientador: Professor Filipe Silva
![Page 2: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/2.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 3: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/3.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 4: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/4.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 5: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/5.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 6: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/6.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 7: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/7.jpg)
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
Estrutura
1
2
3
4
5
6
7
![Page 8: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/8.jpg)
Estrutura
Problema e Objetivos
Setup Experimental
Obtenção dos Dados Inerciais
Obtenção dos Dados Visuais
Integração dos Dados no Filtro de Kalman
Resultados
Conclusões e Trabalho Futuro
1
2
3
4
5
6
7
![Page 9: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/9.jpg)
Enquadramento
• PHUA (Projeto Humanoide da Universidade de Aveiro)
• Humanoide
• 6 Kg; 667 mm; 27 GDL
• Perceção
• 8 sensores de força nos pés
• Camera fire-wire
• 9 IMUs (unidades de medição inercial)
1
2
3
4
5
6
7
![Page 10: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/10.jpg)
Problema
• Desenvolver um algoritmo de equilíbrio baseado no movimento da cabeça de um robô humanóide;
• Medir o movimento da cabeça do humanóide;
1
2
3
4
5
6
7
![Page 11: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/11.jpg)
Problema
• Desenvolver um algoritmo de equilíbrio baseado no movimento da cabeça de um robô humanóide;
• Medir o movimento da cabeça do humanóide;
1
2
3
4
5
6
7
![Page 12: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/12.jpg)
Objetivo
• Melhorar a precisão das medidas, fundindo duas fontes de dados distintas.
1
2
3
4
5
6
7
![Page 13: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/13.jpg)
Setup Experimental
• Sensores Inerciais;
• Sensores Visuais;
• Unidades de Processamento.
1
2
3
4
5
6
7
![Page 14: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/14.jpg)
Setup Experimental• Sensor A• Sensor B• Unidade de Processamento• Camera fire-wire
RAZOR 9DOF - SEN 10736
1
2
3
4
5
6
7
![Page 15: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/15.jpg)
POLOLU - MinIMU9DOF v2
Setup Experimental• Sensor A• Sensor B• Unidade de Processamento• Camera fire-wire
1
2
3
4
5
6
7
![Page 16: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/16.jpg)
Setup Experimental
Arduino UNO R3
• Sensor A• Sensor B• Unidade de Processamento• Camera fire-wire
1
2
3
4
5
6
7
![Page 17: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/17.jpg)
Setup Experimental
Firefly MV-03MTC - Pointgrey
• Sensor A• Sensor B• Unidade de Processamento• Camera fire-wire
1
2
3
4
5
6
7
![Page 18: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/18.jpg)
Setup Experimental
• Dificuldade na criação de experiências;
• Falta de uma “ground truth” de confiança.
1
2
3
4
5
6
7
![Page 19: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/19.jpg)
Setup Experimental
• Alta repetibilidade;
• “ground truth” de confiança;
• Facilidade na criação de experiências.
FANUC LR Mate 200iD
1
2
3
4
5
6
7
![Page 20: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/20.jpg)
1) Fanuc 200iD2) POLOLU - MinIMU9DOF v23) RAZOR 9DOF - SEN 107364) Arduino UNO R35) Fire-wire Camera
1
2
3
4
5
6
7
![Page 21: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/21.jpg)
Setup Experimental
• Como comunicar com o manipulador industrial?
• fanuc_control
• TCP/IP
• ROBCOMM
1
2
3
4
5
6
7
![Page 22: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/22.jpg)
fanuc_control
Verifica Inputs
Inicializa:• ROS• Publishers• Classe dataReceiver
Comunicação TCP/IP entre PC e
manipulador
Classe robotCom
Corre programa da experiência
Pede pelas coordenadas das
juntas e cartesianasPublica as coordenadas
/fanuc_cart
/fanuc_joint
1
2
3
4
5
6
7
![Page 23: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/23.jpg)
fanuc_control
dataReceiver Class
Initializes subscribers:• sensor_receiver_raw• sensor_receiver_filt• points_received
sensor_receiver_raw/topic_raw_data /fanuc_raw
sensor_receiver_filt/topic_filtered_imu /fanuc_filt
points_received/camera/image_rect_color /fanuc_filt
1
2
3
4
5
6
7
![Page 24: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/24.jpg)
Obtenção dos Dados Inerciais
𝜃𝑧
𝜃𝑥
𝜃𝑦
1
2
3
4
5
6
7
![Page 25: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/25.jpg)
Obtenção dos Dados Inerciais
𝜃𝑧𝑘
𝜃𝑥𝑘 𝜃𝑦𝑘
𝜃𝑥𝜃𝑦𝜃𝑧
𝜃𝑥𝑘+1 = 𝜃𝑥𝑘 + Δ𝑡. 𝜃𝑥𝑘𝜃𝑦𝑘+1 = 𝜃𝑦𝑘 + Δ𝑡. 𝜃𝑦𝑘𝜃𝑧𝑘+1 = 𝜃𝑧𝑘 + Δ𝑡. 𝜃𝑧𝑘
1
2
3
4
5
6
7
![Page 26: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/26.jpg)
Obtenção dos Dados Visuais
• Método da Deteção de Blobs;
• Método da Deteção de Features.
1
2
3
4
5
6
7
![Page 27: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/27.jpg)
Deteção de Blobs
1
2
3
4
5
6
7
![Page 28: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/28.jpg)
𝜽𝒚 = 𝟎 °
Deteção de Blobs
1
2
3
4
5
6
7
![Page 29: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/29.jpg)
𝜽𝒚 = 𝟓𝟓 °
Deteção de Blobs
1
2
3
4
5
6
7
![Page 30: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/30.jpg)
Deteção de Blobs
• Medição direta da posição angular (orientação);
• Não depende de medidas previamente efetuadas.
Vantagens
• Incapacidade de operar em certo ambientes.
Desvantagens
1
2
3
4
5
6
7
![Page 31: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/31.jpg)
Deteção de Features
1
2
3
4
5
6
7
![Page 32: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/32.jpg)
1
2
3
4
5
6
7
Deteção de Features
![Page 33: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/33.jpg)
Deteção de Features
[T]
1
2
3
4
5
6
7
![Page 34: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/34.jpg)
𝑇 =𝑐𝑜𝑠𝜃𝑇 − 𝑠𝑖𝑛𝜃𝑇 𝑑𝑧𝑠𝑒𝑛𝜃𝑇 𝑐𝑜𝑠𝜃𝑇 𝑑𝑥
0 0 1
Δ𝜃𝑇𝑘
𝜃𝑦𝑘 = Δ𝜃𝑇𝑘 + 𝜃𝑦𝑘−1
𝜃𝑦𝑘 =Δ𝜃𝑇𝑘Δ𝑡𝑘
+ 𝜃𝑦𝑘−1
Deteção de Features
1
2
3
4
5
6
7
![Page 35: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/35.jpg)
5
6
7
Deteção de Featuresangle_tol
1
2
3
4
![Page 36: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/36.jpg)
Δ𝜃 𝑓𝑒𝑎𝑡𝑢𝑟𝑒1
Δ𝜃 𝑓𝑒𝑎𝑡𝑢𝑟𝑒3
Δ𝜃 𝑓𝑒𝑎𝑡𝑢𝑟𝑒2
≤ 𝑎𝑛𝑔𝑙𝑒_𝑡𝑜𝑙
1
2
3
4
5
6
7
![Page 37: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/37.jpg)
Obtenção de Dados Visuais (Frame)
Deteção e Extração de Features Locais
Matching das featuresdetetadas e das featuresguardadas (anteriores)
Estimação da Matriz de Transformação [T]
Obtenção de valores de referência para posição e
velocidade angular
1
2
3
4
5
6
7
![Page 38: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/38.jpg)
angle_tol > 0Calcular ângulo entre a reta que contem duas
features e o eixo-x.
Comparar com valor obtido no frame anterior
Δ𝜃 ≤ 𝑎𝑛𝑔𝑙𝑒_𝑡𝑜𝑙
Todas as features foram
processadas
Guardar medidaDescartar medida
não sim
não sim
sim nãoSalvar output de todas as medidas efetuadas
Salvar features detetadas no presente frame
1
2
3
4
5
6
7
![Page 39: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/39.jpg)
Obtenção de Dados Visuais (Frame)
Deteção e Extração de Features Locais
Matching das featuresdetetadas e das featuresguardadas (anteriores)
Estimação da Matriz de Transformação [T]
Obtenção de valores de referência para posição e
velocidade angular
angle_tol > 0Calcular ângulo entre a reta que contem duas
features e o eixo-x.
Comparar com valor obtido anterioremente
Δ𝜃 ≤ 𝑎𝑛𝑔𝑙𝑒_𝑡𝑜𝑙
Todas as features
foram processadas
Guardar medidaDescartar medida
não sim
não sim
sim nãoSalvar output de todas as medidas efetuadas
Salvar features detetadas no presente frame
1
2
3
4
5
6
7
![Page 40: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/40.jpg)
• Método robusto que pode operar em variadas circunstâncias;
• Pode ser usado em diferentes processos.
Vantagens
• Depende de medidas prévias.
Desvantagens
Deteção de Features
1
2
3
4
5
6
7
![Page 41: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/41.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 42: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/42.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 43: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/43.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 44: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/44.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 45: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/45.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 46: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/46.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 47: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/47.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 48: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/48.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 49: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/49.jpg)
Percurso da Experiência
1
2
3
4
5
6
7
![Page 50: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/50.jpg)
Integração dos Dados no Filtro de Kalman
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝑦𝑘 = 𝐶. 𝑥𝑘 + 𝑣𝑘
Equação que define o modeloEquação que define a relação entre as medidas efetuadas e as variáveis de estado
1
2
3
4
5
6
7
![Page 51: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/51.jpg)
Integração dos Dados no Filtro de Kalman
Variáveis de Estado
𝑥𝑘 =
𝜃𝑥𝜃𝑦𝜃𝑧 𝜃𝑥 𝜃𝑦 𝜃𝑧
𝒙𝒌 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝑦𝑘 = 𝐶. 𝑥𝑘 + 𝑣𝑘
1
2
3
4
5
6
7
![Page 52: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/52.jpg)
Definição do Modelo:𝜃𝑘 = 𝜃𝑘−1 + 𝜃𝑘−1. ∆𝑡 + 0,5. 𝜃k−1. ∆t
2
𝜃𝑘 = 𝜃𝑘−1 + 𝜃k−1. ∆𝑡
𝜃𝑘−1 = 𝑢𝑘 = 𝟎
( 𝜃𝑘−1 − 𝜃𝑘−2)∆𝑡𝑘
Integração dos Dados no Filtro de Kalman
𝑥𝑘 = 𝑨. 𝑥𝑘−1 +𝑩. 𝑢𝑘−1 +𝑤𝑘
𝑦𝑘 = 𝐶. 𝑥𝑘 + 𝑣𝑘
1
2
3
4
5
6
7
![Page 53: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/53.jpg)
𝐴 =
1 0 0 Δ𝑡 0 00 1 0 0 Δ𝑡00 0 1 0 0 Δ𝑡0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1
𝐵 =0,5∆t2
0,5∆t2
0,5∆t2
𝑥𝑘 = 𝑨. 𝑥𝑘−1 +𝑩. 𝑢𝑘−1 +𝑤𝑘
𝑦𝑘 = 𝐶. 𝑥𝑘 + 𝑣𝑘
Definição do Modelo:𝜃𝑘 = 𝜃𝑘−1 + 𝜃𝑘−1. ∆𝑡 + 0,5. 𝜃k−1. ∆t
2
𝜃𝑘 = 𝜃𝑘−1 + 𝜃k−1. ∆𝑡
Integração dos Dados no Filtro de Kalman
1
2
3
4
5
6
7
![Page 54: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/54.jpg)
𝒙𝒌 = 𝑨. 𝒙𝒌−𝟏 + 𝑩.𝒖𝒌−𝟏 +𝒘𝒌
𝑦𝑘 = 𝐶. 𝑥𝑘 + 𝑣𝑘
𝜃𝑥𝑖𝜃𝑦𝑖𝜃𝑧𝑖 𝜃𝑥𝑖 𝜃𝑦𝑖 𝜃𝑧𝑖
=
1 0 0 Δ𝑡 0 00 1 0 0 Δ𝑡00 0 1 0 0 Δ𝑡0 0 0 1 0 00 0 0 0 1 00 0 0 0 0 1
.
𝜃𝑥𝑖−1𝜃𝑦𝑖−1𝜃𝑧𝑖−1 𝜃𝑥𝑖−1 𝜃𝑦𝑖−1 𝜃𝑧𝑖−1
Integração dos Dados no Filtro de Kalman
+0,5∆t2
0,5∆t2
0,5∆t2
𝜃𝑥𝑖−1 𝜃𝑦𝑖−1 𝜃𝑧𝑖−1
1
2
3
4
5
6
7
![Page 55: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/55.jpg)
𝑦𝑘 =
…
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝒚𝒌 = 𝐶. 𝑥𝑘 + 𝑣𝑘
𝜃𝑧𝑘
𝜃𝑥𝑘
𝜃𝑦𝑘
Integração dos Dados no Filtro de Kalman
1
2
3
4
5
6
7
![Page 56: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/56.jpg)
𝑦𝑘 =
𝜃𝑥 𝜃𝑦 𝜃𝑧
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝒚𝒌 = 𝐶. 𝑥𝑘 + 𝑣𝑘
𝜃𝑧𝑘
𝜃𝑥𝑘
𝜃𝑦𝑘
Integração dos Dados no Filtro de Kalman
1
2
3
4
5
6
7
![Page 57: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/57.jpg)
𝑦𝑘 =
𝜃𝑥𝜃𝑦𝜃𝑧 𝜃𝑥 𝜃𝑦 𝜃𝑧
…
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝒚𝒌 = 𝐶. 𝑥𝑘 + 𝑣𝑘
Integração dos Dados no Filtro de Kalman
1
2
3
4
5
6
7
![Page 58: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/58.jpg)
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝒚𝒌 = 𝐶. 𝑥𝑘 + 𝑣𝑘𝑦𝑘 =
𝜃𝑥𝜃𝑦𝜃𝑧 𝜃𝑥 𝜃𝑦 𝜃𝑧
𝜃𝐶𝑦 𝜃𝐶𝑦…
Integração dos Dados no Filtro de Kalman
1
2
3
4
5
6
7
![Page 59: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/59.jpg)
𝑥𝑘 = 𝐴. 𝑥𝑘−1 + 𝐵. 𝑢𝑘−1 +𝑤𝑘
𝑦𝑘 = 𝑪. 𝑥𝑘 + 𝑣𝑘
𝐶 =
1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 1 0 00 0 0 0 1 00 0 0 0 0 10 1 0 0 0 00 0 0 0 1 0
…
Integração dos Dados no Filtro de Kalman
𝑦𝑘 =
𝜃𝑥𝜃𝑦𝜃𝑧 𝜃𝑥 𝜃𝑦 𝜃𝑧
𝜃𝐶𝑦 𝜃𝐶𝑦…
𝑥𝑘 =
𝜃𝑥𝜃𝑦𝜃𝑧 𝜃𝑥 𝜃𝑦 𝜃𝑧
1
2
3
4
5
6
7
![Page 60: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/60.jpg)
Integração dos Dados no Filtro de Kalman
*1
𝐺𝑘 =
………………………………………………………………………………………………0 0 0 0 0 00 0 0 0 0 0
I I I
V
I I I
V
I I I
V
I I I
V
I I I
V
MD MDMD MDMDMD
Kalman Filter
*1
1
2
3
4
5
6
7
![Page 61: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/61.jpg)
Resultados
1
2
3
4
5
6
7
![Page 62: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/62.jpg)
ResultadosExperiência 1 (sem erro)
𝐸𝑟𝑟𝑜 𝑀é𝑑𝑖𝑜 = 𝜃𝑦𝑔𝑟𝑜𝑢𝑛𝑑𝑡𝑟𝑢𝑡ℎ − 𝜃𝑦𝑚𝑒𝑑𝑖𝑑𝑜/𝑛𝑚𝑒𝑑𝑖çõ𝑒𝑠 [°]
1
2
3
4
5
6
7
![Page 63: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/63.jpg)
ResultadosExperiência 1 (sem erro)
Err
o M
édio
[º]
Inercial Visual KF(u=0) KF(u≠0)
0,0
00
,50
1,0
01,
502
,00
2,5
0
![Page 64: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/64.jpg)
ResultadosExperiência 1 (com erro)
Err
o M
édio
[º]
Inercial Visual KF(u=0) KF(u≠0)
0,0
00
,50
1,0
01,
502
,00
2,5
0
![Page 65: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/65.jpg)
ResultadosExperiência 2 (sem erro)
Err
o M
édio
[º]
Inercial Visual KF(u=0) KF(u≠0)
0,0
00
,50
1,0
01,
502
,00
2,5
0
![Page 66: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/66.jpg)
ResultadosExperiência 2 (com erro)
Err
o M
édio
[º]
Inercial Visual KF(u=0) KF(u≠0)
0,0
00
,50
1,0
01,
502
,00
2,5
0
![Page 67: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/67.jpg)
Conclusões
• FANUC 200iD fornece uma “ground truth” de alta confiança;
• Fundir dados inerciais e visuais vai resultar em dados com mais confiança do que usando os sensores individualmente;
• O filtro de Kalman tem a capacidade de eliminar ruído;
• As abordagens e métodos utilizados, são extensíveis a outros casos.
1
2
3
4
5
6
7
![Page 68: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/68.jpg)
Trabalho Futuro
• Desenvolver um modelo do filtro para situações em tempo real;
• Aplicar método a diferentes conjuntos de informação;
• Desenvolver estado de variáveis que descrevam o humanoide;
• Completar programa fanuc_control.
1
2
3
4
5
6
7
![Page 69: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/69.jpg)
1
2
3
4
5
6
7
Peixoto, J., Santos, V., & Silva, F. (2016) Proprioceptive visual tracking of a humanoidrobot head motion. Lecture Notes in ComputerScience, 9730 LNCS.
![Page 70: Integração de Dados Visuais e Inerciais Para o …lars.mec.ua.pt/public/LAR Projects/Humanoid/2016...Problema e Objetivos Setup Experimental Obtenção dos Dados Inerciais Obtenção](https://reader031.fdocumentos.com/reader031/viewer/2022022718/5c5b878209d3f254368bd5b7/html5/thumbnails/70.jpg)
Integração de Dados Visuais e Inerciais Para o
Equilíbrio de Um Robô HumanóideJoão Carlos Pimentel Fidalgo Peixoto
60140
Universidade de Aveiro,Departamento de Engenharia Mecânica
Orientador: Professor Vítor SantosCoorientador: Professor Filipe Silva