Transcrição

1 UMA SISTEMA DE RASTREAMENTO DE POSE PARA APLICAÇÕES EM TELEIMERSÃO Cláudio dos Santos Fernandes, Mario Fernando Montenegro Campos, Armando Alves Neto Universidade Federal de Minas Gerais Belo Horizonte, Minas Gerais, Brasil s: Abstract The present work copes with the development of one of the modules that compose a system for tele-immersion applications responsible for providing the user with stereoscopic visual information from a remote environment acquired with a controlled video camera pair in fronto-parallel disposition. The user s head orientation is obtained from an Inertial Measurement Unit IMU, whose orientation is sent and matched by the remote camera pair. The user visualizes the remote environment via a tridimensional visual interface that exhibits the scene captured by the remote camera pair. The system was developed and implemented with lowcost cameras and servos, presenting a performance compaptible with current tele-immersion applications. Keywords Tele-immersion, Inertial Measurement Unit, Attitude Estimation. Resumo O presente trabalho aborda o desenvolvimento de um dos módulos de um sistema para aplicações de tele-imersão responsável por prover ao usuário informação visual estereoscópica de um ambiente remoto adquirida por um par de câmeras na configuração fronto-paralela controladas. A pose da cabeça do usuário é obtida por uma unidade de medição inercial IMU (Inertial Measurement Unit), cuja orientação é enviada e correspondida pelo par de câmeras remoto. O usuário visualiza o ambiente remoto por meio de uma interface visual tridimensional que exibe a cena capturada pelo par remoto de câmeras. O sistema foi desenvolvido e implementado com câmeras e servos de baixo custo, apresentando desempenho compatível com aplicações de tele-imersão correntes. Keywords Tele-imersão, Unidade de Medição Inercial, Estimação de Pose. 1 Introdução Alguns dos problemas estudados na área da tele-robótica são relacionados ao manuseio remoto de robôs manipuladores (como aqueles empregados em cirurgias à distância) e robôs móveis (tais como os robôs de prospecção). Podem ser empregados recursos de tele-presença, onde a interface utilizada pelo usuário controlador possui elementos imersivos, que dão a este usuário a sensação de presença no ambiente em que o robô está inserido. Dentre as diversas aplicações da tele-robótica, destacam-se as recentes pesquisas relacionadas a cirurgia à distância. Nesta área, os requisitos em termos de interação entre operador e dispositivos cirúrgicos e a precisão do procedimento são tais que até mesmo aspectos tácteis do procedimento devem ser transmitidas para o cirurgião remoto (Bar-Cohen et al., 2001; Gosselin et al., 2005). Outra linha de pesquisa promissora é a busca e salvamento empregando robôs tele-manipulados. A idéia do uso de robôs como agentes intermediários de sondagem é bastante atraente em cenários onde a presença humana se mostra inviável, o que ocorre durante a exploração de ambientes muito distantes, hostis ou de difícil alcance. Como foi demonstrado em (Martins and Ventura, 2009), o uso de técnicas imersivas pode aumentar o grau de percepção de detalhes dos cenários sendo remotamente explorados, o que demonstra o potencial desta área. Pesquisas que se utilizam de sensores inerciais (IMU Inertial Measurement Unit) têm sido realizadas com aplicações em diversas áreas do conhecimento. Dentre alguns exemplos pode-se citar os estudos em biomecânica, em que o monitoramento do movimento humano é uma das grandes áreas de aplicação (Luinge and Veltink, 2005). A utilização de IMUs como equipamentos estabilizadores de câmeras tem crescido nos últimos anos e atraído a atenção de pesquisadores para o desenvolvimento de filtros de sinais digitais com capacidade de atenuação de ruídos cada vez maior (Madgwick, 2010). As IMUs são, também, largamente aplicadas em sistemas de navegação autônoma, tanto no processo de odometria, quanto na obtenção de atitude de veículos aéreos (Wendel et al., 2006). Neste trabalho, um arcabouço para a combinação dessas técnicas foi desenvolvido e aplicado em um protótipo com dois graus de liberdade controlada por um IMU de baixo custo. O protótipo foi desenvolvido com as seguintes funcionalidades: Visão em primeira pessoa: O usuário tem à sua disposição uma interface que o permita visualizar o mundo através da perspectiva do robô, conforme ilustra a Figura 1. Controle de movimentos pan-tilt imersivo: O controle sobre a orientação das câmeras instaladas no dispositivo é feito automaticamente em sincronismo com a movimentação do usuário capturada por uma IMU. Exibição da imagem do manipulador em uma ISSN: Vol. X 1037

2 X SBAI Simpósio Brasileiro de Automação Inteligente tela anexa ao robo : Uma ca mera posicionada em frente ao usua rio transmite o vı deo ao vivo para o robo, que exibe as imagens recebidas em um projetor anexo em sua parte frontal. versa o e realizada pelas equac o es 1. m12 m11 sin(θ) m13 m23 tan(φ) m33 tan(ψ) (1) Em que: Figura 1: IMU e Interface visual utilizada no projeto. A IMU foi acoplada a Head Mounted Display para que seja estimada a pose da cabec a do usua rio. Este trabalho esta dividido em cinco sec o es. A sec a o 2 apresenta uma fundamentac a o teo rica sobre representac a o de rotac o es com quate rnios. O me todo proposto para o ca lculo da pose da IMU e proposto na sec a o 4. Na sec a o 5 sa o apresentados e discutidos os resultados dos experimentos realizados para a calibrac a o da IMU utilizada. As concluso es sa o discutidas na sec a o 6. 2 ψ A ngulo de guinada, Θ A ngulo de arfagem, φ A ngulo de rolagem, m11 2u2 + 2x2 1, m12 2xy + 2uz, m13 2uy 2xz, m23 2yz + 2ux, m33 2u2 + 2z 2 1. (2) As equac o es 2 sa o derivadas de tal forma que os a ngulos de Euler correspondam a rotac o es na ordem ψ, Θ, φ. Esta ordem se deve ao fato de que a orientac a o do objeto sendo rotacionado (topo do dispositivo ilustrado pela Figura 3) e obtida por meio de duas rotac o es tal que a rotac a o em torno do eixo vertical precede a do eixo horizontal. Representac a o de Rotac a o Existem algumas maneiras de se descrever a rotac a o de corpos rı gidos. Descritos em 1843 pelo matema tico irlande s William Rowan Hamilton, os quate rnios sa o uma extensa o ao domı nio dos nu meros complexos que sa o amplamente utilizados para modelar rotac o es. Um quate rnio pode ser escrito na forma q u+xi+yj +zk, em que u, x, y e z sa o nu meros reais e i, j e k sa o unidades imagina rias. Os quate rnios tambe m sa o comumente expressos na forma vetorial q [u, ~n], em que u e sua parte real e ~n sa o os coeficientes dos seus componentes imagina rios i, j e k. A sua aplicabilidade no problema da representac a o de rotac o es pode ser facilmente compreendido ao se pensar neste tipo de operac a o como um deslocamento angular em torno de um eixo. Pode ser demonstrado (Kuipers, 2002) que, dados dois quate rnios qa [cos(ω), sin(ω)~r] e qb [0, ~v ], o produto qa qb q a resultara em um vetor v~0 que representa a rotac a o de ω unidades do vetor ~v em torno do eixo ~r, em que q a e o conjugado do quate rnio qa e e a operac a o de produto entre quate rnios. Em algumas situac o es especı ficas, e necessa rio mudar da representac a o utilizando quate rnios para a representac a o em a ngulos de Euler. Conforme demonstrado em (Kuipers, 2002), essa con- ISSN: Vol. X Figura 3: Eixos de rotac a o do dispositivo construı do. Observe que ele possui dois graus de liberdade ortogonais o primeiro vertical e o segundo, horizontal. 3 Metodologia A IMU empregada neste trabalho e constituı da de tre s acelero metros mutuamente ortogonais o mesmo valendo para seus magneto metros e girosco pios, totalizando nove sensores. Quando um acelero metro ou magneto metro e colocado em repouso, a sua saı da e dada por r s + ρ, em que ρ e um ruı do gaussiano branco e s e proporcional a componente do campo medido paralela ao plano do sensor. Pode ser observado por meio da Figura 4 que s cos(θ). Como s e uma varia vel discreta, o valor de cos(θ) pode ser aproximado por meio da relac a o

3 Figura 2: Esquema de uma interface completamente tele-imersiva, em que informações audiovisuais são transmitidas em ambos os sentidos, e informações hapticas são enviadas ao operador. O escopo deste trabalho é restrito ao cálculo da pose do usuário e à transmissão de audio/vídeo entre dispositivo e operador. m y θ m s m x Figura 4: Saída s de um sensor em função da sua posição relativa à grandeza medida. cos(θ) s τ σ (3) Em que τ (offset) é o valor retornado pelo sensor quando θ 90 e σ (ganho) é calculado por (s max s min )/2. Se, por exemplo, um sensor medisse uma grandeza cujos valores mínimo e máximo de 412 e 612, respectivamente, a faixa de medidas seria de 200 unidades (e, portanto, σ 200/2 100) e τ 512. A combinação de três sensores perpendiculares entre si é utilizada para medir os componentes x, y e z de um vetor r paralelo à grandeza amostrada. A próxima etapa no cálculo da orientação da IMU em ângulos de Euler é obter o quatérnio q tal que q jq m e q kq a, em que j [1, 0, 0], k [0, 0, 1] e m e a são os valores observados dos magnetômetros e acelerômetros, respectivamente. É importante entender o porquê de se medirem dois vetores referenciais. Com apenas um vetor medido por exemplo, o campo magnético não seria possível saber, dado um m qualquer, se a IMU está virada para cima ou para baixo. De forma mais adequada, o papel da segunda medição (no exemplo dado, o acelerômetro) é determinar o grau de rotação da IMU em torno do eixo em que m pertence. Por outro lado, não são necessários mais do que dois vetores perpendiculares para resolver o problema de determinação da orientação em um espaço tridimensional. De fato, o terceiro vetor poderia ser obtido por meio do produto vetorial a m. Um dos problemas em se lidar com magnetômetros é que nem sempre o valor de m é perpendicular à gravidade. Como resultado, a orientação de m com relação a a não será determinística; pior: há regiões do planeta em que o campo magnético possui a mesma direção do campo gravitacional, tornando impossível nestas regiões a tarefa de obter a orientação completa da IMU com o método proposto. Nas demais regiões, o problema da não perpendicularidade entre m e a pode ser resolvido ao se trabalhar com a componente de m perpendicular a a, conforme mostra a Figura 5. Esta componente é dada pela equação 4. m ( a m) a (4) a Figura 5: Obtenção do componente de m perpendicular a a. A decomposição do vetor m resulta da premissa de que, em um IMU de baixo custo, os acelerômetros são muito mais confiáveis do que os magnetômetros. Uma vez encontrado o vetor m, o problema inicial se reduz ao problema de encontrar o quatérnio q tal que: m m q m q j, q a q k. Este problema pode ser decomposto em duas etapas. A primeira consiste em encontrar um quatérnio q 1 cuja rotação, ao ser aplicada a a retorna k. Aplicando esta rotação também a m, ISSN: Vol. X 1039

4 j m a θ 1 k a m (a) Eixos da IMU e eixos referenciais, estado inicial. i j k a θ 2 m a m i (b) Após a primeira rotação, o eixo a é restaurado. k a dos sensores, é necessário que se realize um préprocessamento antes que o valor final seja enviado aos servo-motores. Assumindo-se linearidade, utiliza-se um filtro de Kalman para estimar os valores finais. As equações dos modelos de predição e atualização são descritas, respectivamente, por 6 e 7: Θ t Θ t 1 + ω Θt 1 dt, ψ t ψ t 1 + ω ψt 1 dt, ω Θt ω Θt 1, ω ψt ω ψt 1. (6) j m i a m (c) Após a segunda rotação, todos os eixos coincidem. Figura 6: Passos para a determinação do quatérnio q. tem-se a configuração da figura 6(b). Para calcular o quatérnio q 1, será necessário obter o ângulo θ 1 entre a e k e o eixo de rotação e 1 entre estes. O valor de θ 1 pode ser calculado pela fórmula do produto escalar, enquanto e 1 é dado pelo produto vetorial de a e k: a k a k cos(θ 1 ), ( a θ 1 arccos k ) a, k e 1 a k, q 1 [cos(θ 1 /2), sin(θ 1 /2) e 1 ]. Na segunda etapa, deve ser encontrado o quatérnio q 2 que, ao rotacionar q 1 m q 1, resulta em j. Seja m q 1 m q 1, as mesmas relações utilizadas para o cálculo de q 1 podem ser empregadas para que seja determinado q 2 : ( ) m j θ 2 arccos, m j e 2 m j, q 2 [cos(θ 2 /2), sin(θ 2 /2) e 2 ]. Uma vez aplicada a rotação de q 2 ao sistema resultante da rotação de q 1, a orientação obtida será a mesma ilustrada pela Figura 6(c). Com estes valores calculados, é possível então determinar o valor de q, o qual é dado pela equação 5. q q 2 q 1 (5) Uma vez obtido o quatérnio q, é possível obter os ângulos de Euler por meio das equações 2. Devido ao processo de obtenção desses valores e do ruído tipicamente presente nos sinais de saída α Θ, β ψ, Ω α ω Θ, Ω β ω ψ. (7) Em que α e β são os ângulos de Euler obtidos de q referentes à arfagem e à guinada, respectivamente. As variáveis Ω α e Ω β são as velocidades angulares relativas a α e a β e foram obtidas dos girômetros. Como pode ser percebido, esta modelagem é baseada na idéia de que as velocidades angulares permanecem relativamente constantes ao longo do tempo. A constante dt é o período de amostragem dos sinais da IMU. 4 Experimentos A validação da operação do sistema foi realizada por meio de vários experimentos. Para isto, foi utilizada a IMU 6DoF v4 fabricada pela Sparkfun. Ela é composta de acelerômetros Freescale MMA7260Q com 800±60 mv/g, com sensibilidade de 1.5g (suportando até 6g com menores resoluções); magnetômetros Honeywell HMC1051Z 1, 0±0, 2 mv/gauss e girômetros InvenSense IDG /seg. A saída analógica dos acelerômetros e girômetros é quantizada em 10 bits, com valores variando de 0 a A taxa de amostragem de todos os sensores é de 120 Hz. O processo de experimentação realizado consistiu da estimação dos parâmetros σ e τ de todos os magnetômetros e acelerômetros. Os experimentos foram feitos com a IMU afixada a um servomotor, onde foi rotacionada em voltas, cada qual com um período de 10 segundos. Este procedimento foi repetido para cada sensor, sendo que as rotações para amostragem dos magnetômetros foram feitas em torno do eixo vertical, enquanto amostragens dos acelerômetros foram feitas em torno de um eixo horizontal arbitrário. A Figura 7 ilustra a saída crua do magnetômetro y para o primeiro ciclo. ISSN: Vol. X 1040

5 Intensidade da saída Saída do magnetômetro y em função do tempo x y z τ σ τ σ τ σ Acelerômetro Magnetômetro Tabela 1: Resultados da calibração dos sensores Magnetômetros e Acelerômetros Tempo (s) Figura 7: Amostragem da saída de um dos magnetômetros em uma rotação de 360 com o sensor paralelo ao campo magnético da terra. Intensidade da saída Regressão não linear para cálculo dos parâmetros σ e τ do magnetômetro y. τ Saída filtrada Curva estimada A atenuação do ruído de todos os sinais foi feita com um filtro passa-baixas de Butterworth (Oppenheim et al., 1999) de terceira ordem, com coeficientes 0, 04 (magnetômetros) e 0, 2 (acelerômetros). A filtragem mais agressiva é feita nos magnetômetros, uma vez que seu ruído é significativamente maior do que o observado nos acelerômetros. A figura 8 ilustra o resultado da filtragem da amostragem observada na Figura 7. Intensidade da saída Saída filtrada do magnetômetro y em função do tempo Tempo (s) Figura 8: Resultado da aplicação do filtro de Butterworth na amostra da Figura 7. A aproximação de τ e σ foi feita por meio de uma regressão não linar da equação 8 aos sinais filtrados. Durante este processo, são estimados os parâmetros σ, τ, a (proporcional à orientação inicial do sensor) e b (período efetivo da rotação). A Figura 9 ilustra o resultado do ajuste da equação 8 ao gráfico da Figura 8. Os valores encontrados para todos os sensores estão compilados na Tabela 1. ( y(x) σ cos 2π x + a b ) + τ (8) Tempo (s) Figura 9: Regressão não linear para aproximação dos parâmetros τ e σ do magnetômetro y. Com relação aos acelerômetros, as formas de onda obtidas durante os experimentos são influenciadas pela soma dos componentes gravidade, aceleração centrípeta de cada sensor e ruído gaussiano atenuado. Nestes experimentos, a aceleração centrípeta foi considerada desprezível para fins de simplificação. Pode ser observado que a sensibilidade destes sensores é suficiente para representar os cossenos dos ângulos [0, 180] em, pelo menos, 504 valores distintos, o que pode representar um erro de até 6. Quanto aos magnetômetros, as medições do campo magnético terrestre são expressas com apenas cerca de 30 valores distintos, ou seja, rotações de até 22 podem não produzir uma resposta significativa na saída destes sensores. 5 Conclusões O pré-processamento dos sinais emitidos pelos acelerômetros e magnetômetros com o filtro de Butterworth aumentou significativamente a sua relação sinal/ruído, tornando viável a aplicação da técnica desenvolvida para combinar estes dados a fim de obter a orientação da IMU no espaço. Uma das propostas de trabalhos futuros é a utilização de um filtro de kalman não linear em conjunto com esta técnica com o objetivo de obter estimativas estatisticamente ótimas para estes dados. Como é demonstrado por (Madgwick, 2010), é possível estabelecer um modelo que resulte em uma filtragem suave dos sinais sem penalizar o tempo de resposta às movimentações da IMU. Outro trabalho importante a ser realizado é a calibração interna dos σ ISSN: Vol. X 1041

6 acelerômetros e magnetômetros para que sejam determinados os ângulos de desalinhamento entre os sensores da IMU, de forma que seja possível realinhá-los em uma das etapas do pipeline de processamento destes sinais. Com relação ao par estéreo de câmeras, é necessário utilizar uma técnica de retificação e estabilização das imagens obtidas, uma vez que estas câmeras não são perfeitamente alinhadas e estão sujeitas a vibrações devido aos movimentos do robô. Wendel, J., Meister, O., Schlaile, C. and Trommer, G. F. (2006). An integrated gps/memsimu navigation system for an autonomous helicopter, Aerospace Science and Technology 10(6): Agradecimentos Este trabalho contou com o apoio de grande parte da equipe do Laboratório de Visão Computacional e Robótica (VeRLab) da Universidade Federal de Minas Gerais e com o financiamento das agências CNPq, CAPES e FAPEMIG. Referências Bar-Cohen, Y., Mavroidis, C., Bouzit, M., Dolgin, B., Harm, D. L., Kopchok, G. E. and White, R. (2001). Virtual reality robotic telesurgery simulations using memica haptic system, Proceedings of SPIE s 8th Annual International Symposium on Smart Structures and Materials. Gosselin, F., Bidard, C. and Brisset, J. (2005). Design of a high fidelity haptic device for telesurgery, Proceedings of the 2005 IEEE International Conference on Robotics and Automation pp Kuipers, J. B. (2002). Quaternions and Rotation Sequences: A Primer With Applications to Orbits, Aerospace, and Virtual Reality, Princeton University Press. Luinge, H. J. and Veltink, P. H. (2005). Measuring orientation of human body segments using miniature gyroscopes and accelerometers, Medical and Biological Engineering and Computing pp Madgwick, S. O. (2010). An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Martins, H. and Ventura, R. (2009). Immersive 3 d teleoperation of a search and rescue robot using a head mounted display, Proceedings of the IEEE. Oppenheim, A. V., Schafer, R. W. and Buck, J. R. (1999). Discrete time signal processing, 2nd ed. edn, Prentice Hall, Inc. ISSN: Vol. X 1042