DESENVOLVIMENTO, INTERFACE E SIMULAÇÃO HARDWARE-IN-THE-LOOP DE UM MECANISMO PARA ESTABILIZAÇÃO DE CÂMERAS EM ROBÔS SUBMARINOS Rodrigo Costa, Andrei Battistel, e Liu Hsu Programa de Engenharia Elétrica COPPE/UFRJ, C.P. 68504 21945-970-Rio de Janeiro, RJ, Brasil Email: rodrigocosta@poli.ufrj.br, battistel@ufrj.br,liu@coep.ufrj.br Abstract Stabilized platforms are used in robotics, telecommunication and military systems where it is needed to inertially stabilize an object mounted on a moving base. The objective is to isolate objects as cameras, sensors or weapons from motion to which the host vehicle is submitted, in order to keep the objects pointing to a chosen direction. To achieve this goal, inertial sensors are used to measure the vehicle orientation, and mechanical assemblies are employed to correct the object line-of-sight. This work implements a stabilized platform in order to provide a framework for camera stabilization development to the underwater robot LUMA. Interface between all system components and real-time applications, as well as hardware-in-the-loop simulation are also dealt in this work. Keywords hardware-in-the-loop simulation, stabilized platforms Resumo Plataformas estabilizadas são sistemas utilizados em robótica, telecomunicações e sistemas militares onde se deseja estabilizar inercialmente um determinado objeto montado sobre uma base móvel. O objetivo é, através de uma medida da orientação e de um mecanismo de compensação mecânica, manter objetos como câmeras, sensores ou armamentos apontando em uma determinada direção. Neste trabalho, trata-se da implementação de uma plataforma estabilizada voltada para o desenvolvimento de um sistema de estabilização de câmeras para o robô submarino LUMA. São tratados ainda o desenvolvimento de uma interface para comunicação dos componentes em tempo real bem como a utilização de simulação hardware-in-the-loop do sistema. Keywords simulação hardware-in-the-loop, plataformas estabilizadas 1 Introdução Plataformas inercialmente estabilizadas são sistemas utilizados em diversas áreas de engenharia para estabilizar e apontar câmeras, sensores ou armamentos colocados sobre uma base móvel, e.g. um robô ou veículo. A estabilização consiste em manter a invariante em relação ao referencial inercial (Terra) a direção para a qual o objeto a ser estabilizado aponta na presença de movimentos da base. Esta direção é representada por um vetor e chamada de linha de visada. Para atingir este objetivo, utilizam-se sensores capazes de obter a orientação da base e mecanismos atuados para corrigir a atitude do objeto a ser estabilizado (Hilkert, 2008). A medida da orientação é proveniente da fusão de dados de giroscópicos, e/ou acelerômetros e magnetômetros. O sistema de atuadores mecânicos de compensação mais comumente usado é um gimbal de dois eixos ortogonais (Masten, 2008). Neste caso, a compensação se dá pelo cálculo de dois ângulos, cujas combinações permitem apontar para qualquer direção. Entre as aplicações de plataformas estabilizadas encontradas na literatura encontram-se câmeras em automóveis (Guenthner et al., 2007) e aviões (Hurak and Rezac, 2009); armamentos sobre veículos militares (Guo and Tan, 2010); apontamento de antenas (Karabinis et al., 1988) e estabilização de lentes em câmeras portáteis (Hilkert, 2008). A motivação deste trabalho é a obtenção de um sistema capaz de estabilizar câmeras no robô submarino LUMA, exibido na Fig. 1. Trata-se de um ROV (Remotely Operated Vehicle (Hsu et al., 2000)) utilizado pelo Programa Antártico Brasileiro (PROANTAR). As expedições realizadas na Antártida mostraram dificuldades em obter imagens nítidas do ambiente submarino, uma vez que o robô é sujeito a perturbações externas, como corrente submarinas, além do movimento causado à câmera em decorrência das próprias manobras intencionais. Conforme observa (Chaumette and Crétual, 2000), a captação de imagens em ambientes submarinos é dificultada ainda pela falta de contraste e atenuação da luz, de forma que a movimentação da câmera dificulta ainda mais a aquisição de imagens. Neste trabalho, busca-se obter um sistema a partir do qual se possa testar diferentes técnicas de estabilização e fornecer uma plataforma de estabilização de câmeras voltado para aplicação no ROV LUMA (GSCAR/PEE/COPPE). Foram desenvolvidos uma plataforma experimental composta por uma unidade inercial para medição da orientação e por Pan & Tilt como atuador mecânico, uma interface para comunicação destes equipamentos em tempo real utilizando MAT- LAB/Simulink, além de um modelo 3D para simulação hardware-in-the-loop do equipamento. São apresentados resultados experimentais e de simu- ISSN: 2175-8905 - Vol. X 971
Figura 1: Robô submarino LUMA Figura 3: Foto da Plataforma Estabilizada lação. A representação da orientação utilizada é a de ângulos de Euler na ordem XYZ, ou comumente referida como roll-pitch-yaw (Sciavicco et al., 2009), e está representada na Fig. 2. Quando se trata dos ângulos de compensação, estes são referidos como pan e tilt ou azimute e elevação. Em ambos os casos, são rotações ao redor dos eixos z e y (yaw e pitch). Esta nomenclatura é adotada a fim de distinguir os movimentos externos do ROV dos ângulos de correção do mecanismo de compensação. quenos de roll, uma vez que a base larga dificulta este tipo de rotação. Matematicamente, o problema de estabilização pode ser colocado como os ângulos de azimute (pan) α e elevação (tilt) γ que compensam a orientação da câmera em relação ao referencial inercial, indicados na Fig. 4. O cálculo destes ângulos é relativamente simples e pode ser encontrado em (Battistel et al., 2010), de forma que a formulação aqui utilizada segue a metodologia lá apresentada. Figura 2: Roll, pitch e yaw do LUMA 2 Formulação Geral do Problema A plataforma projetada para experimentos de estabilização consiste em um Pan & Tilt, uma unidade inercial, e uma plataforma de dois graus de liberdade confeccionada em madeira. Sobre o Pan & Tilt foi montada uma câmera compacta a fim de observar os resultados na imagem final. Na Fig. 3 o protótipo pode ser visto e, como se pode ver, o sensor inercial é posicionado na base da plataforma, caracterizando um algoritmo de estabilização indireta (Kennedy and Kennedy, 2003). Esta escolha foi adotada aqui uma vez que no robô LUMA são utilizados dados da unidade inercial do ROV e, portanto, as medições disponíveis são da orientação do robô e não da câmera. Note também que dada a geometria da plataforma, espera-se apenas movimentos pe- Figura 4: Ângulos de compensação e sistemas de coordenadas Na Fig. 4, os sistemas de coordenadas a, b e L indicam os sistemas de referência da câmera, do veículo onde a plataforma é colocada, e o sistema inercial, respectivamente. Os ângulos α e γ são os ângulos de correção de pan e tilt, conforme indicado. O vetor da linha de visada desejada é denotado por U L e assume-se que este é escolhido pelo operador ao ativar a estabilização em uma determinada posição. Basicamente, os ângulos de compensação são obtidos da matriz de orientação da plataforma em relação ao inercial, denotada aqui por C bl (ψ, θ, φ) SO(3) e dependem da direção para a qual se quer apontar, denotada por U L IR3, de forma que: C T bl(ψ, θ, φ)u L = [x 1 x 2 x 3 ] T (1) ISSN: 2175-8905 - Vol. X 972
e os ângulos de compensação são dados por ( ) x2 α = arctan ; γ = arcsin( x 3 ) (2) x 1 A matriz de orientação C bl é obtida diretamente do sensor inercial. O cálculo da correção é realizado por um PC que obtém as medições do sensor e comanda o Pan & Tilt através de uma aplicação em tempo real, descrita na seção 3. Estes ângulos α e γ permitem que a linha de visada da câmera se mantenha invariante, uma vez que esta é a primeira coluna da matriz de orientação C al, conforme (Wen and Kreutz- Delgado, 1991), i.e., U L = C al e x = C bl (ψ, θ, φ)r z (α)r y (γ) e x (3) sendo e T x = [1, 0, 0]. Uma interface em tempo real utilizando as ferramentas Real Time Workshop e Real Time Window s Target para desempenhar esta tarefa foi desenvolvida para realizar experimentos de estabilização. 3 Comunicação entre PC, Sensor e Pan/Tilt O Pan & Tilt PTU-D46 Directed Perception é um dispositivo que rotaciona em dois eixos distintos, pan e tilt, utilizando motores de passo. Os comandos de posição e velocidade dos eixos são realizada por meio de comunicação via RS-232 ao controlador da unidade. A posição angular dos eixos pan e tilt é controlada por uma seqüência de entradas de posições discretas, de modo que cada posição equivale a um passo do motor e corresponde a um deslocamento angular de 0, 0514285. Os ângulos de compensação azimute α e elevação γ em radianos são então transformados em posições. A unidade de controle gera tensões nos motores de passo dos eixos, que então movem para posição desejada. O controlador também informa a posições e velocidades atuais dos eixos, assim possibilitando fechar um malha de controle. A unidade inercial 3DM-X da Microstrain possui um microcontrolador que combina os dados dos giroscópios, acelerômetros e magnetômetros para fornecer a orientação. A comunicação com o PC é feita também via RS-232 e é possível adquirir a orientação diretamente em ângulos de Euler giro-estabilizados. Os ângulos de Euler giroestabilizado fornecem uma estimativa precisa de orientação, mesmo que o sensor esteja é exposto a acelerações lineares transitórias, ou interferência magnética. Os ângulos Roll e Yaw têm um alcance 180 a 180. O ângulo Pitch tem um alcance de 70 a 70. Utilizando o Matalb/Simulink e as ferramentas Real-Time Workshop (RTW) e Real-Time Windows Target (RTWT) foi desenvolvido o software para comunicação serial com os dispositivos Figura 5: Sistema Hardware-in-the-loop. e um ambiente de testes em tempo real no sistema operacional Windows. O Real-Time Windows Target fornece uma biblioteca de drivers para comunicação com dispositivos externos e um kernel de tempo real. O Real-Time Workshop gera e compila um programa em C a partir da representação do modelo desenvolvido no Simulink. 4 Simulação Hardware-in-the-loop Devido a complicações mecânicas com o Pan & Tilt, um modelo de simulação hardware-in-theloop foi utilizado a fim de testar os algoritmos de estabilização. Esta é uma técnica utilizada no desenvolvimento e testes de sistemas embarcados em tempo real, e consiste em uma simulação em cuja malha se inserem componentes reais. Assim, é possível testar não só o hardware como também o software do sistema, sendo possível avaliar fatores como a possibilidade de trabalho em tempo real, robustez a ruídos, atrasos e a dinâmicas não modeladas. Um típico sistema hardware-in-the-loop é composto de i) modelos da planta, dos sensores e atuadores, uma vez que estes geralmente contém características não-lineares; ii) PC target, sistemas embarcados que executam os modelos em tempo real; e iii) PC host, onde se fornece uma interface gráfica para o usuário; e do iv) hardware, um componente real inserido na malha. A Fig. 5 mostra o representação esquemática da simulação hardware-in-the-loop. Neste trabalho, o componente real é o sensor inercial montado sobre a plataforma. Um computador atua como host/target, visto que além de fornecer a interface gráfica para controle da simulação, simula um ambiente de sistema embarcado realizando a comunicação com o sensor. O Pan & Tilt será representado por um modelo matemático. 4.1 Modelagem Matemática do Pan & Tilt A fim de prover um modelo preciso para simulação, o Pan & Tilt foi modelado de acordo com especificações técnicas e experimentos realizados no laboratório. A Fig. 6 mostra a resposta do equipamento a uma entrada em degrau de posição angular. ISSN: 2175-8905 - Vol. X 973
5 Resultados Experimentais e Aspectos Práticos Figura 6: Pan/Tilt Resposta ao degrau do motor do Note que trata-se de uma rampa, indicando que o sistema pode ser bem representado por um integrador. Tal fato pode ser justificado pela presença de controladores internos ao equipamento, uma vez que este atende a comandos de posição e velocidade. Foram acrescentados ainda saturações que representam os limites físicos de alcance do equipamento. Para o Pan, o alcance varia entre 159 e 159, enquanto para o Tilt os limites são 47 e 31. Uma vez que se tratam de motores de passo, um quantizador é incluído a fim de reproduzir a característica discreta do movimento, cuja resolução é 0.0514 /posição. 4.2 Interface de comunicação e controle O computador é responsável por adquirir a orientação do sensor e calcular os ângulos de compensação em tempo real. Para tal, foram desenvolvidas aplicações específicas para garantir a operação utilizando as ferramentas Real-Time Workshop e Real-Time Windows Target. A orientação medida pelo sensor é adquirida via RS-232 utilizando os blocos Real-Time Windows Target e uma C S-Function, formando o subsistema Giros. A dinâmica do Pan & Tilt é representada na malha de simulação como o modelo matemático de dois motores para cada eixo. Além disso para visualização do sistema, um modelo virtual do Pan & Tilt foi criado utilizando o toolbox de Realidade Virtual do Simulink para fornecer uma representação 3D do equipamento cujo movimento reproduza a dinâmica do componente modelado. O diagrama de blocos do sistema pode ser visto na Fig. 7. Figura 7: Diagrama de Blocos Sistema Hardwarein-the-loop. Uma vez calculados os ângulos de correção, uma lógica é empregada no comando do mecanismo a fim de diminuir a trepidação ocasionada pela característica discreta do sistema. Um algoritmo de ganho variável foi implementado dividindo o controle em três intervalos, de forma que o ganho é maior para erros maiores de posição e nulo para erros menores do que cinco posições (0.2571 ). Sendo θ o ângulo de compensação e θ o ângulo medido: θ = ρ θ θ sign(θ θ) (4) onde ρ é um parâmetro que varia com o erro conforme: 5 se θ θ a 3 se b θ ρ = θ < a 2 se c θ (5) θ < b 0 se θ θ < c. Os valores das constantes a, b, c, bem como os valores de ρ foram escolhidos após diversos testes e fixados. Tem-se, no experimento a = 60, b = 20, c = 5. Foram obtidos resultados de simulação hardware-in-the-loop para situações distintas de movimento externo, quando i) há apenas movimento de pitch; ii) quando são feitos movimentos de roll, pitch e yaw; e iii) quando o movimento externo é de amplitude superior ao alcance do mecanismo. Em todos os casos busca-se apontar na horizontal, isto é U L = [1 0 0] T. 5.1 Estabilização de pitch No primeiro teste procurou-se obter uma estabilização de pitch apenas. A Fig. 8 mostra os movimentos medidos na plataforma e pode-se observar que há um pequeno movimento de yaw. Este fato é compreensível, uma vez que é difícil obter movimentos sem nenhum grau de acoplamento nos diferentes graus de liberdade. Ainda assim, no canal de roll o movimento é desprezível. Os ângulos de compensação podem ser visualizados na Fig. 9 e pode-se notar que a característica de elevação é similar à de pitch, enquanto o perfil do ângulo de azimute é similar ao de yaw. Tal fato é esperado, uma vez que nestas condições, para variação de pitch apenas e apontando na horizontal, a compensação é direta e desacoplada. Observando-se apenas os ângulos de compensação é difícil perceber a precisão da estabilização. Esta pode ser melhor observada na Fig. 10, onde se observa o erro angular na linha de visada. Esta grandeza corresponde ao ângulo entre a direção desejada e a obtida na estabilização, e pode-se observar que o erro máximo é inferior a 3. Esta é uma precisão adequada para o ROV, uma vez que ISSN: 2175-8905 - Vol. X 974
Figura 8: Movimentos da plataforma: roll (ψ), pitch (θ) e yaw (φ) - Teste 01 Figura 11: Movimentos da Plataforma: Roll (ψ), Pitch (θ), Yaw (φ) para o segundo teste na presença de movimentos mais bruscos e de maior amplitude do ROV. Figura 9: Ângulos de compensação α (a) e γ (b) - Teste 01 os alvos normalmente estão suficientemente próximos. Ainda, trata-se de uma precisão que permite manter o objeto procurado no campo de visão. Figura 10: Módulo do erro da linha de visada - Teste 01 Figura 12: Ângulos de compensação α (a) e γ (b) - Teste 02 5.3 Limitações físicas Um teste adicional foi realizado a fim de observar aspectos práticos do sistema, como por exemplo a limitação física de alcance dos ângulos do Pan & Tilt. Na Fig. 13 observa-se o resultado da compensação quando o yaw da plataforma é maior do que o ângulo máximo de azimute. Estas limitações foram incluídas no modelo, de forma que o resultado obtido prevê um erro maior nestas condições. O erro da linha de visada em radioanos é determinado por e rad = arccos(ul U L), onde U L é a linha de visada medida, definida na equação (3), e UL é a linha de visada desejada. 5.2 Estabilização de roll, pitch e yaw O segundo teste apresentado consiste na estabilização nos três graus de liberdade, como se vê na Fig. 11. Note que os movimentos são de amplitudes bastante razoáveis, da ordem de dezenas de graus. O erro na linha de visada é visto na Fig. 12. Note que o erro é similar ao caso anterior, abaixo de 3, indicando uma precisão adequada mesmo Figura 13: Ângulo Desejado α, Ângulo do Mecanismo e Erro Este resultado é interessante pois mostra um ponto a ser investigado: uma vez que se busca ISSN: 2175-8905 - Vol. X 975
apontar para alvos além dos limites de alcance da câmera, a orientação do robô pode ser controlada em uma malha externa, reorientando-o a fim de manter a compensação da câmera dentro de seus limites de operação. Esta condição é ainda mais severa na prática, uma vez que o campo de visão da câmera pode ser restrito por partes do próprio robô. 6 Conclusões Ao fim deste trabalho, obteve-se um sistema capaz de integrar um sensor inercial e um mecanismo mecânico para estabilização de uma câmera sujeita a movimentos externos de uma plataforma. Os resultados de simulação hardware-in-the-loop mostram que o sistema é capaz de estabilizar a câmera com um erro aceitável. Embora não tenha sido possível utilizar o Pan & Tilt até o final do projeto, o desenvolvimento da interface e da plataforma experimental possibilitou a obtenção de um sistema experimental através do qual se pode testar outras técnicas de estabilização. Além disso, o mecanismo mecânico pode ser substituído em trabalhos futuros, de forma a utilizar a interface desenvolvida. A implementação em temporeal possibilitou também a verificação da aplicabilidade de um sistema de estabilização de câmeras no ROV LUMA. Agradecimentos Os autores agradecem ao PROANTAR/CNPq e à CAPES pelo apoio financeiro a este projeto e aos prof. Fernando Lizarralde e Ramon Costa pelo apoio e motivação em desenvolver este trabalho. Hilkert, J. (2008). Inertially stabilized platform technology: Concepts and principles, IEEE Control System Magazine 28(1): 26 46. Hsu, L., Costa, R., Lizarralde, F. and Cunha, J. P. V. S. (2000). Passive arm based dynamic positioning system for remotely operated underwater vehicles, IEEE Robotics&Automation Magazine. Hurak, Z. and Rezac, M. (2009). Combined line-of-sight inertial stabilization and visual tracking: application to airborne camera platform, 48th IEEE Conference on Decision and Control 48. Karabinis, P. D., Egri, R. G. and Bennet, C. L. (1988). Antenna pointing and scanning control for a two axis gimbal system in the presence of platform motion, IEEE. Kennedy, P. J. and Kennedy, R. L. (2003). Direct versus indirect line of sight (los) stabilization, IEEE Transactions on Control Systems Technology 11(1): 3 15. Masten, M. (2008). Inertially stabilized platforms for optical imaging systems, IEEE Control System Magazine 28(1): 47 64. Sciavicco, L., Siciliano, B., Villani, L. and Oriolo, G. (2009). Robotics: modelling, planning and control, Springer Verlag. Wen, J. T. and Kreutz-Delgado, K. (1991). The attitude control problem, IEEE Trans. Aut. Contr. 36(10): 1148 1162. Referências Battistel, A., Lizarralde, F. and Hsu, L. (2010). Estabilização de plataformas inerciais utilizando apenas duas medidas de sensores giroscópicos, Congresso Brasileiro de Automática. Chaumette, F. and Crétual, A. (2000). Dynamic stabilization of a pan and tilt camera for submarine image visualization, Computer Vision and Image Understanding 79: 47 65. Guenthner, W., Glasauer, S., Wagner, P. and Ulbrich, H. (2007). Biomimetic control for adaptive camera stabilization in driverassistance systems, Journal of Mechanical Science and Technology 21: 930 934. Guo, L.-D. and Tan, Z.-F. (2010). *application of a sliding mode variable structure controller for stabilized platform of shipborne weapons, IEEE Control System Magazine 30(1): 12 14. ISSN: 2175-8905 - Vol. X 976