X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 971

DESENVOLVIMENTO, INTERFACE E SIMULACAO HARDWARE-IN-THE-LOOPDE UM MECANISMO PARA ESTABILIZACAO DE CAMERAS EM ROBOS

SUBMARINOS

Rodrigo Costa, Andrei Battistel, e Liu Hsu∗

∗Programa de Engenharia EletricaCOPPE/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 isneeded 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 achosen direction. To achieve this goal, inertial sensors are used to measure the vehicle orientation, and mechanicalassemblies are employed to correct the object line-of-sight. This work implements a stabilized platform in orderto provide a framework for camera stabilization development to the underwater robot LUMA. Interface betweenall system components and real-time applications, as well as hardware-in-the-loop simulation are also dealt inthis work.

Keywords— hardware-in-the-loop simulation, stabilized platforms

Resumo— Plataformas estabilizadas sao sistemas utilizados em robotica, telecomunicacoes e sistemas militaresonde se deseja estabilizar inercialmente um determinado objeto montado sobre uma base movel. O objetivo e,atraves de uma medida da orientacao e de um mecanismo de compensacao mecanica, manter objetos comocameras, sensores ou armamentos apontando em uma determinada direcao. Neste trabalho, trata-se da im-plementacao de uma plataforma estabilizada voltada para o desenvolvimento de um sistema de estabilizacao decameras para o robo submarino LUMA. Sao tratados ainda o desenvolvimento de uma interface para comunicacaodos componentes em tempo real bem como a utilizacao de simulacao hardware-in-the-loop do sistema.

Keywords— simulacao hardware-in-the-loop, plataformas estabilizadas

1 Introducao

Plataformas inercialmente estabilizadas sao sis-temas utilizados em diversas areas de engenhariapara estabilizar e apontar cameras, sensores ouarmamentos colocados sobre uma base movel, e.g.um robo ou veıculo. A estabilizacao consiste emmanter a invariante em relacao ao referencial iner-cial (Terra) a direcao para a qual o objeto a serestabilizado aponta na presenca de movimentos dabase. Esta direcao e representada por um vetor echamada de linha de visada.

Para atingir este objetivo, utilizam-se sen-sores capazes de obter a orientacao da base emecanismos atuados para corrigir a atitude doobjeto a ser estabilizado (Hilkert, 2008). A me-dida da orientacao e proveniente da fusao de dadosde giroscopicos, e/ou acelerometros e magnetome-tros. O sistema de atuadores mecanicos de com-pensacao mais comumente usado e um gimbal dedois eixos ortogonais (Masten, 2008). Neste caso,a compensacao se da pelo calculo de dois angulos,cujas combinacoes permitem apontar para qual-quer direcao.

Entre as aplicacoes de plataformas estabi-lizadas encontradas na literatura encontram-secameras em automoveis (Guenthner et al., 2007)e avioes (Hurak and Rezac, 2009); armamentossobre veıculos militares (Guo and Tan, 2010);apontamento de antenas (Karabinis et al., 1988)e estabilizacao de lentes em cameras portateis

(Hilkert, 2008).

A motivacao deste trabalho e a obtencao deum sistema capaz de estabilizar cameras no robosubmarino LUMA, exibido na Fig. 1. Trata-sede um ROV (Remotely Operated Vehicle (Hsuet al., 2000)) utilizado pelo Programa Antar-tico Brasileiro (PROANTAR). As expedicoes re-alizadas na Antartida mostraram dificuldades emobter imagens nıtidas do ambiente submarino,uma vez que o robo e sujeito a perturbacoesexternas, como corrente submarinas, alem domovimento causado a camera em decorrencia dasproprias manobras intencionais. Conforme ob-serva (Chaumette and Cretual, 2000), a captacaode imagens em ambientes submarinos e dificultadaainda pela falta de contraste e atenuacao da luz,de forma que a movimentacao da camera dificultaainda mais a aquisicao de imagens.

Neste trabalho, busca-se obter um sistema apartir do qual se possa testar diferentes tecni-cas de estabilizacao e fornecer uma plataforma deestabilizacao de cameras voltado para aplicacaono ROV LUMA (GSCAR/PEE/COPPE). Foramdesenvolvidos uma plataforma experimental com-posta por uma unidade inercial para medicaoda orientacao e por Pan & Tilt como atuadormecanico, uma interface para comunicacao destesequipamentos em tempo real utilizando MAT-LAB/Simulink, alem de um modelo 3D para simu-lacao hardware-in-the-loop do equipamento. Saoapresentados resultados experimentais e de simu-

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 972

Figura 1: Robo submarino LUMA

lacao.

A representacao da orientacao utilizada ea de angulos de Euler na ordem XYZ, ou co-mumente referida como roll-pitch-yaw (Sciaviccoet al., 2009), e esta representada na Fig. 2.Quando se trata dos angulos de compensacao,estes sao referidos como pan e tilt ou azimute eelevacao. Em ambos os casos, sao rotacoes ao re-dor dos eixos z e y (yaw e pitch). Esta nomen-clatura e adotada a fim de distinguir os movimen-tos externos do ROV dos angulos de correcao domecanismo de compensacao.

Figura 2: Roll, pitch e yaw do LUMA

2 Formulacao Geral do Problema

A plataforma projetada para experimentos de es-tabilizacao consiste em um Pan & Tilt, umaunidade inercial, e uma plataforma de dois grausde liberdade confeccionada em madeira. Sobre oPan & Tilt foi montada uma camera compacta afim de observar os resultados na imagem final.

Na Fig. 3 o prototipo pode ser visto e,como se pode ver, o sensor inercial e posicionadona base da plataforma, caracterizando um al-goritmo de estabilizacao indireta (Kennedy andKennedy, 2003). Esta escolha foi adotada aquiuma vez que no robo LUMA sao utilizados da-dos da unidade inercial do ROV e, portanto, asmedicoes disponıveis sao da orientacao do robo enao da camera.

Note tambem que dada a geometria daplataforma, espera-se apenas movimentos pe-

Figura 3: Foto da Plataforma Estabilizada

quenos de roll, uma vez que a base larga dificultaeste tipo de rotacao.

Matematicamente, o problema de estabiliza-cao pode ser colocado como os angulos de azimute(pan) α e elevacao (tilt) γ que compensam a ori-entacao da camera em relacao ao referencial iner-cial, indicados na Fig. 4. O calculo destes angulose relativamente simples e pode ser encontrado em(Battistel et al., 2010), de forma que a formulacaoaqui utilizada segue a metodologia la apresentada.

Figura 4: Angulos de compensacao e sistemas decoordenadas

Na Fig. 4, os sistemas de coordenadas a, b eL indicam os sistemas de referencia da camera,do veıculo onde a plataforma e colocada, e o sis-tema inercial, respectivamente. Os angulos α e γsao os angulos de correcao de pan e tilt, conformeindicado. O vetor da linha de visada desejada edenotado por UL e assume-se que este e escolhidopelo operador ao ativar a estabilizacao em umadeterminada posicao.

Basicamente, os angulos de compensacao saoobtidos da matriz de orientacao da plataformaem relacao ao inercial, denotada aqui porCbL(ψ, θ, φ) ∈ SO(3) e dependem da direcao paraa qual se quer apontar, denotada por U∗

L ∈ IR3,de forma que:

CTbL(ψ, θ, φ)U∗

L = [x1 x2 x3]T (1)

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 973

e os angulos de compensacao sao dados por

α = arctan

(x2x1

); γ = arcsin(−x3) (2)

A matriz de orientacao CbL e obtida direta-mente do sensor inercial. O calculo da correcao erealizado por um PC que obtem as medicoes dosensor e comanda o Pan & Tilt atraves de umaaplicacao em tempo real, descrita na secao 3.

Estes angulos α e γ permitem que a linhade visada da camera se mantenha invariante,uma vez que esta e a primeira coluna da matrizde orientacao CaL, conforme (Wen and Kreutz-Delgado, 1991), i.e.,

UL = CaL ex = CbL(ψ, θ, φ)Rz(α)Ry(γ) ex (3)

sendo eTx = [1, 0, 0].Uma interface em tempo real utilizando as fer-

ramentas Real Time Workshop e Real Time Win-dow s Target para desempenhar esta tarefa foi de-senvolvida para realizar experimentos de estabi-lizacao.

3 Comunicacao entre PC, Sensor ePan/Tilt

O Pan & Tilt PTU-D46 Directed Perception e umdispositivo que rotaciona em dois eixos distintos,pan e tilt, utilizando motores de passo. Os coman-dos de posicao e velocidade dos eixos sao realizadapor meio de comunicacao via RS-232 ao contro-lador da unidade. A posicao angular dos eixos pane tilt e controlada por uma sequencia de entradasde posicoes discretas, de modo que cada posicaoequivale a um passo do motor e corresponde a umdeslocamento angular de 0, 0514285◦. Os angulosde compensacao azimute α e elevacao γ em ra-dianos sao entao transformados em posicoes. Aunidade de controle gera tensoes nos motores depasso dos eixos, que entao movem para posicao de-sejada. O controlador tambem informa a posicoese velocidades atuais dos eixos, assim possibili-tando fechar um malha de controle.

A unidade inercial 3DM-X da Microstrainpossui um microcontrolador que combina os da-dos dos giroscopios, acelerometros e magnetome-tros para fornecer a orientacao. A comunicacaocom o PC e feita tambem via RS-232 e e possıveladquirir a orientacao diretamente em angulos deEuler giro-estabilizados. Os angulos de Euler giro-estabilizado fornecem uma estimativa precisa deorientacao, mesmo que o sensor esteja e expostoa aceleracoes lineares transitorias, ou interferen-cia magnetica. Os angulos Roll e Yaw tem umalcance −180◦ a 180◦. O angulo Pitch tem umalcance de −70◦ a 70◦.

Utilizando o Matalb/Simulink e as ferramen-tas Real-Time Workshop (RTW) e Real-TimeWindows Target (RTWT) foi desenvolvido o soft-ware para comunicacao serial com os dispositivos

Figura 5: Sistema Hardware-in-the-loop.

e um ambiente de testes em tempo real no sis-tema operacional Windows. O Real-Time Win-dows Target fornece uma biblioteca de driverspara comunicacao com dispositivos externos e umkernel de tempo real. O Real-Time Workshop gerae compila um programa em C a partir da repre-sentacao do modelo desenvolvido no Simulink.

4 Simulacao Hardware-in-the-loop

Devido a complicacoes mecanicas com o Pan &Tilt, um modelo de simulacao hardware-in-the-loop foi utilizado a fim de testar os algoritmos deestabilizacao. Esta e uma tecnica utilizada no de-senvolvimento e testes de sistemas embarcados emtempo real, e consiste em uma simulacao em cujamalha se inserem componentes reais. Assim, epossıvel testar nao so o hardware como tambemo software do sistema, sendo possıvel avaliar fa-tores como a possibilidade de trabalho em temporeal, robustez a ruıdos, atrasos e a dinamicas naomodeladas.

Um tıpico sistema hardware-in-the-loop ecomposto de i) modelos da planta, dos sensorese atuadores, uma vez que estes geralmente con-tem caracterısticas nao-lineares; ii) PC target, sis-temas embarcados que executam os modelos emtempo real; e iii) PC host, onde se fornece umainterface grafica para o usuario; e do iv) hardware,um componente real inserido na malha.

A Fig. 5 mostra o representacao esquematicada simulacao hardware-in-the-loop. Neste tra-balho, o componente real e o sensor inercial mon-tado sobre a plataforma. Um computador atuacomo host/target, visto que alem de fornecer a in-terface grafica para controle da simulacao, simulaum ambiente de sistema embarcado realizando acomunicacao com o sensor. O Pan & Tilt serarepresentado por um modelo matematico.

4.1 Modelagem Matematica do Pan & Tilt

A fim de prover um modelo preciso para simula-cao, o Pan & Tilt foi modelado de acordo comespecificacoes tecnicas e experimentos realizadosno laboratorio. A Fig. 6 mostra a resposta doequipamento a uma entrada em degrau de posicaoangular.

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 974

Figura 6: Resposta ao degrau do motor doPan/Tilt

Note que trata-se de uma rampa, indicandoque o sistema pode ser bem representado por umintegrador. Tal fato pode ser justificado pela pre-senca de controladores internos ao equipamento,uma vez que este atende a comandos de posicao evelocidade.

Foram acrescentados ainda saturacoes querepresentam os limites fısicos de alcance doequipamento. Para o Pan, o alcance varia entre−159◦ e 159◦, enquanto para o Tilt os limites sao−47◦ e 31◦. Uma vez que se tratam de motoresde passo, um quantizador e incluıdo a fim de re-produzir a caracterıstica discreta do movimento,cuja resolucao e 0.0514◦/posicao.

4.2 Interface de comunicacao e controle

O computador e responsavel por adquirir a ori-entacao do sensor e calcular os angulos de com-pensacao em tempo real. Para tal, foram desen-volvidas aplicacoes especıficas para garantir a ope-racao utilizando as ferramentas Real-Time Work-shop e Real-Time Windows Target. A orientacaomedida pelo sensor e adquirida via RS-232 uti-lizando os blocos Real-Time Windows Target euma C S-Function, formando o subsistema Giros.A dinamica do Pan & Tilt e representada namalha de simulacao como o modelo matematicode dois motores para cada eixo. Alem disso paravisualizacao do sistema, um modelo virtual do Pan& Tilt foi criado utilizando o toolbox de RealidadeVirtual do Simulink para fornecer uma represen-tacao 3D do equipamento cujo movimento repro-duza a dinamica do componente modelado. O di-agrama de blocos do sistema pode ser visto naFig. 7.

Figura 7: Diagrama de Blocos Sistema Hardware-in-the-loop.

5 Resultados Experimentais e AspectosPraticos

Uma vez calculados os angulos de correcao, umalogica e empregada no comando do mecanismo afim de diminuir a trepidacao ocasionada pela ca-racterıstica discreta do sistema. Um algoritmo deganho variavel foi implementado dividindo o con-trole em tres intervalos, de forma que o ganho emaior para erros maiores de posicao e nulo paraerros menores do que cinco posicoes (0.2571◦).Sendo θ∗ o angulo de compensacao e θ o angulomedido:

θ = ρ ∗ |θ∗ − θ| ∗ sign(θ∗ − θ) (4)

onde ρ e um parametro que varia com o erro con-forme:

ρ =

5 se |θ∗ − θ| ≥ a3 se b ≥ |θ∗ − θ| < a2 se c ≥ |θ∗ − θ| < b0 se |θ∗ − θ| < c

(5)

. Os valores das constantes a, b, c, bem como osvalores de ρ foram escolhidos apos diversos testese fixados. Tem-se, no experimento a = 60, b = 20,c = 5.

Foram obtidos resultados de simulacaohardware-in-the-loop para situacoes distintas demovimento externo, quando i) ha apenas movi-mento de pitch; ii) quando sao feitos movimentosde roll, pitch e yaw ; e iii) quando o movimentoexterno e de amplitude superior ao alcance do me-canismo. Em todos os casos busca-se apontar nahorizontal, isto e UL = [1 0 0]T .

5.1 Estabilizacao de pitch

No primeiro teste procurou-se obter uma estabi-lizacao de pitch apenas. A Fig. 8 mostra os movi-mentos medidos na plataforma e pode-se obser-var que ha um pequeno movimento de yaw. Estefato e compreensıvel, uma vez que e difıcil obtermovimentos sem nenhum grau de acoplamento nosdiferentes graus de liberdade. Ainda assim, nocanal de roll o movimento e desprezıvel.

Os angulos de compensacao podem ser visu-alizados na Fig. 9 e pode-se notar que a caracte-rıstica de elevacao e similar a de pitch, enquantoo perfil do angulo de azimute e similar ao de yaw.Tal fato e esperado, uma vez que nestas condicoes,para variacao de pitch apenas e apontando na ho-rizontal, a compensacao e direta e desacoplada.

Observando-se apenas os angulos de compen-sacao e difıcil perceber a precisao da estabilizacao.Esta pode ser melhor observada na Fig. 10, ondese observa o erro angular na linha de visada. Estagrandeza corresponde ao angulo entre a direcaodesejada e a obtida na estabilizacao, e pode-se ob-servar que o erro maximo e inferior a 3◦. Esta euma precisao adequada para o ROV, uma vez que

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 975

Figura 8: Movimentos da plataforma: roll (ψ),pitch (θ) e yaw (φ) - Teste 01

Figura 9: Angulos de compensacao α (a) e γ (b)- Teste 01

os alvos normalmente estao suficientemente proxi-mos. Ainda, trata-se de uma precisao que permitemanter o objeto procurado no campo de visao.

Figura 10: Modulo do erro da linha de visada -Teste 01

O erro da linha de visada em radioanos e de-terminado por erad = arccos(U∗

L · UL), onde UL ea linha de visada medida, definida na equacao (3),e U∗

L e a linha de visada desejada.

5.2 Estabilizacao de roll, pitch e yaw

O segundo teste apresentado consiste na estabi-lizacao nos tres graus de liberdade, como se ve naFig. 11. Note que os movimentos sao de ampli-tudes bastante razoaveis, da ordem de dezenas degraus.

O erro na linha de visada e visto na Fig. 12.Note que o erro e similar ao caso anterior, abaixode 3◦, indicando uma precisao adequada mesmo

Figura 11: Movimentos da Plataforma: Roll (ψ),Pitch (θ), Yaw (φ) para o segundo teste

na presenca de movimentos mais bruscos e demaior amplitude do ROV.

Figura 12: Angulos de compensacao α (a) e γ (b)- Teste 02

5.3 Limitacoes fısicas

Um teste adicional foi realizado a fim de observaraspectos praticos do sistema, como por exemplo alimitacao fısica de alcance dos angulos do Pan &Tilt. Na Fig. 13 observa-se o resultado da com-pensacao quando o yaw da plataforma e maior doque o angulo maximo de azimute. Estas limitacoesforam incluıdas no modelo, de forma que o resul-tado obtido preve um erro maior nestas condicoes.

Figura 13: Angulo Desejado α, Angulo do Meca-nismo e Erro

Este resultado e interessante pois mostra umponto a ser investigado: uma vez que se busca

X SBAI – Simpósio Brasileiro de Automação Inteligente18 a 21 de setembro de 2011São João del-Rei - MG - Brasil

ISSN: 2175-8905 - Vol. X 976

apontar para alvos alem dos limites de alcance dacamera, a orientacao do robo pode ser controladaem uma malha externa, reorientando-o a fim demanter a compensacao da camera dentro de seuslimites de operacao. Esta condicao e ainda maissevera na pratica, uma vez que o campo de visaoda camera pode ser restrito por partes do propriorobo.

6 Conclusoes

Ao fim deste trabalho, obteve-se um sistema capazde integrar um sensor inercial e um mecanismomecanico para estabilizacao de uma camera su-jeita a movimentos externos de uma plataforma.Os resultados de simulacao hardware-in-the-loopmostram que o sistema e capaz de estabilizar acamera com um erro aceitavel. Embora nao tenhasido possıvel utilizar o Pan & Tilt ate o finaldo projeto, o desenvolvimento da interface e daplataforma experimental possibilitou a obtencaode um sistema experimental atraves do qual sepode testar outras tecnicas de estabilizacao. Alemdisso, o mecanismo mecanico pode ser substituıdoem trabalhos futuros, de forma a utilizar a inter-face desenvolvida. A implementacao em tempo-real possibilitou tambem a verificacao da aplicabi-lidade de um sistema de estabilizacao de camerasno ROV LUMA.

Agradecimentos

Os autores agradecem ao PROANTAR/CNPq e aCAPES pelo apoio financeiro a este projeto e aosprof. Fernando Lizarralde e Ramon Costa peloapoio e motivacao em desenvolver este trabalho.

Referencias

Battistel, A., Lizarralde, F. and Hsu, L. (2010).Estabilizacao de plataformas inerciais uti-lizando apenas duas medidas de sensoresgiroscopicos, Congresso Brasileiro de Au-tomatica .

Chaumette, F. and Cretual, A. (2000). Dynamicstabilization of a pan and tilt camera for sub-marine image visualization, Computer Visionand Image Understanding 79: 47–65.

Guenthner, W., Glasauer, S., Wagner, P. andUlbrich, H. (2007). Biomimetic controlfor adaptive camera stabilization in driver-assistance systems, Journal of MechanicalScience and Technology 21: 930–934.

Guo, L.-D. and Tan, Z.-F. (2010). *application ofa sliding mode variable structure controllerfor stabilized platform of shipborne weapons,IEEE Control System Magazine 30(1): 12–14.

Hilkert, J. (2008). Inertially stabilized platformtechnology: Concepts and principles, IEEEControl System Magazine 28(1): 26–46.

Hsu, L., Costa, R., Lizarralde, F. and Cunha, J. P.V. S. (2000). Passive arm based dynamic po-sitioning system for remotely operated under-water vehicles, IEEE Robotics&AutomationMagazine .

Hurak, Z. and Rezac, M. (2009). Combinedline-of-sight inertial stabilization and visualtracking: application to airborne cameraplatform, 48th IEEE Conference on Decisionand Control 48.

Karabinis, P. D., Egri, R. G. and Bennet, C. L.(1988). Antenna pointing and scanning con-trol for a two axis gimbal system in the pres-ence of platform motion, IEEE .

Kennedy, P. J. and Kennedy, R. L. (2003). Directversus indirect line of sight (los) stabilization,IEEE Transactions on Control Systems Tech-nology 11(1): 3–15.

Masten, M. (2008). Inertially stabilized platformsfor optical imaging systems, IEEE ControlSystem Magazine 28(1): 47–64.

Sciavicco, L., Siciliano, B., Villani, L. and Oriolo,G. (2009). Robotics: modelling, planning andcontrol, Springer Verlag.

Wen, J. T. and Kreutz-Delgado, K. (1991). Theattitude control problem, IEEE Trans. Aut.Contr. 36(10): 1148–1162.