PONTIFÍCIA UNIVERSIDADE CATÓLICA DO RIO GRANDE DO SUL FACULDADE DE ENGENHARIA

Transcrição

1 PONTIFÍCIA UNIVERSIDADE CATÓLICA DO RIO GRANDE DO SUL FACULDADE DE ENGENHARIA INSTRUMENTAÇÃO DE UMA PLATAFORMA DE STEWART PARA VALIDAÇÃO DE ESTIMADORES DE POSIÇÃO E ORIENTAÇÃO BASEADOS EM FUSÃO DE SENSORES Porto Alegre, 27 de novembro de Autor: José Vinícius Melo Pontifícia Universidade Católica do Rio Grande do Sul Curso de Engenharia Elétrica Av. Ipiranga 6681, - Prédio 30 - CEP: Porto Alegre - RS - Brasil jose.vinicius@acad.pucrs.br Orientador: Prof. Aurélio Tergolina Salton Pontifícia Universidade Católica do Rio Grande do Sul Av. Ipiranga 6681, - Prédio 30 - Bloco A - Sala CEP: Porto Alegre - RS- Brasil aurelio.salton@pucrs.br

2

3 RESUMO Este trabalho aborda os métodos utilizados para a instrumentação de uma plataforma de Stewart utilizada na validação de um estimador de posição e orientação para sistemas com seis graus de liberdade. O estimador baseia-se na fusão de sensores um giroscópio, um acelerômetro e uma câmera através de um Filtro de Kalman Estendido. Os modelos utilizados para o sistema e os sensores são baseados na representação por quatérnios. Os métodos utilizados para a instrumentação e aquisição de dados são detalhados, assim como as técnicas utilizadas para o processamento dos dados da câmera através da biblioteca OpenCV. Por fim, a cinemática direta é utilizada para validar os modelos dos sensores e os resultados da estimativa de posição e orientação obtidos com a aplicação do filtro. Palavras-chave: Fusão de Sensores, Filtro de Kalman Estendido, Quatérnios, Processamento de Imagens. ABSTRACT This work discusses the methods used in the instrumentation of a Stewart platform used to validate a pose estimator to six degrees of freedom systems. The estimator is based on a sensor fusion of a gyroscope, an accelerometer and a camera through an Extended Kalman Filter. The models used for the system and sensors are based in quaternions. The approach used in instrumentation and data acquisition is detailed, as well as the techniques applied to camera data processing through the OpenCV library. Finally, the forward kinematic is used to validate the sensor models and the pose results from filter application. Key-words: Sensor Fusion, Extended Kalman Filter, Quaternions, Image Processing.

4

5 Toda a nossa ciência, comparada com a realidade, é primitiva e infantil - e, no entanto, é a coisa mais preciosa que temos. Albert Einstein ( )

6

7 SUMÁRIO 1 INTRODUÇÃO Motivações Objetivos Estrutura REFERENCIAL TEÓRICO Plataforma de Stewart Filtro de Kalman Estendido Quatérnios Modelos do Giroscópio e do Acelerômetro Modelo da Câmera METODOLOGIA Instrumentação e Aquisição de Dados Validação dos Modelos Filtro de Kalman Estendido APLICAÇÃO E RESULTADOS Instrumentação e Aquisição de Dados Módulo Central Atuadores Sensores Inerciais Processamento de Imagem e Aquisição dos Marcadores Validação dos Modelos Estimativa da Posição e Orientação através da Cinemática Direta Simulação dos Sensores Filtro de Kalman Estendido CONCLUSÃO APÊNCIDES... 35

8 6.1 Apêndice A Apêndice B Apêndice C REFERÊNCIAS... 42

9 1 INTRODUÇÃO O estudo de sistemas dinâmicos com seis graus de liberdade (Six Degrees of Freedom 6-DOF) é de suma importância para a engenharia, pois abrange uma vasta gama de processos e são utilizados em diversas áreas. Eles são úteis em muitas aplicações, como, por exemplo, simuladores de voo (STEWART, 1965), braços robóticos (KUMRA, SAXENA e MEHTA, 2012), manipuladores paralelos (DE FARIA, 2016), etc. A plataforma de Stewart, por exemplo, é um manipulador paralelo com 6 atuadores lineares e pode ser usada para garantir a estabilidade na transferência de cargas entre portos ou grandes embarcações para navios menores, reduzindo o tempo e custo envolvidos, além de garantir maior segurança (MELLO, 2011). Sendo assim, são de grande interesse para a indústria e o meio acadêmico, permitindo a aplicação de diversas técnicas de controle. Sistemas 6-DOF são estudados há diversas décadas em busca de técnicas de controle eficazes e robustas, portanto, a determinação de sua posição e orientação é essencial. Normalmente, a posição e orientação são inferidas através de sensores inerciais (Inertial Measurement Unit - IMU), como, por exemplo, acelerômetros, giroscópios e magnetômetros. Para tal, são usados sensores de alta precisão e com preços muito elevados. Sensores mais comuns e baratos podem ser uma alternativa, contudo, o drift causado pelo processo de integração dos erros numéricos faz o sinal divergir ao longo do tempo. Uma possível solução para o uso dessas unidades inerciais é a utilização de outros sensores através de técnicas de correção ou fusão de sinais (XU e LI, 2007). Este trabalho considera a fusão do sinal proveniente de uma IMU com uma câmera através do uso do Filtro de Kalman Estendido (Extended Kalman Filter - EKF). O EKF é uma das técnicas mais utilizadas para a fusão e filtragem de sinais, pois fornece um método de predição e correção, minimizando a covariância do erro de estimação. No passo de predição, o modelo do sistema é utilizado para inferir a saída desejada e, posteriormente, a correção é realizada através dos sensores. O EKF tem sido largamente utilizado para a fusão de sensores inerciais e sensores de imagem. Em (NÜTZI, WEISS, et al., 2011), é feita a estimativa de escala na técnica de mapeamento e localização simultânea utilizada em veículos aéreos autônomos, eliminando a necessidade da utilização de duas câmeras. Em (ZHANG, FANG, et al., 2017), é realizada a estimativa de posição e orientação de raquetes de tênis, possibilitando que robôs utilizados como oponentes em treinos tenham uma resposta mais rápida e acurada, facilitando o controle envolvido. 1

10 Para a aplicação do EKF é necessário representar o sistema através de seu modelo matemático linearizado. Para o modelamento de um corpo rígido, esses modelos normalmente possuem diversas não-linearidades que precisam ser simplificadas, podendo comprometer a efetividade do filtro. Uma alternativa é a representação desse modelo através do uso de quatérnios, onde diversas não-linearidades são eliminadas. Quatérnios são uma extensão dos números complexos e fornecem uma forma simplificada e compacta para representar a orientação e translação de um corpo rígido, por isso são largamente utilizados em diversas áreas, como, por exemplo, computação gráfica (BAEK, JEON, et al., 2017). 1.1 Motivações A principal motivação para realizar este trabalho é atuar em conjunto com pesquisadores do Grupo de Automação e Controle de Sistemas (GACS) no desenvolvimento de um estimador de posição e orientação de sistemas 6-DOF baseado em fusão de sensores. Este projeto permite a aplicação de diversos conteúdos abordados durante a graduação, como, por exemplo, desenvolvimento de hardware e firmware, bem como o aprendizado de novas técnicas através do uso de processamento de imagens e o estudo de estimadores. Como consequência, é possível unir teoria e prática, levando a um aperfeiçoamento das técnicas adquiridas ao longo do curso de Engenharia Elétrica. 1.2 Objetivos Tendo em vista o contexto abordado, o principal objetivo deste trabalho é realizar a instrumentação e sensoriamento de uma plataforma de Stewart, que será utilizada para validação de um estimador de posição e orientação de sistemas 6-DOF através da fusão de uma IMU e uma câmera utilizando um Filtro de Kalman Estendido baseado na representação por quatérnios. A fusão de sensores utiliza dados provenientes de um acelerômetro, de um giroscópio e dados de uma câmera, onde a projeção nas imagens de marcadores com posição conhecida está relacionada com a posição e orientação da câmera, situada no topo da plataforma. A plataforma de Stewart utilizada pertence ao GACS, situada na sala 219 do bloco A do prédio de Engenharia da Pontifícia Universidade Católica do Rio Grande do Sul (PUCRS). Para atingir os objetivos propostos, as seguintes metas foram traçadas: 1. Estudo e compreensão do Filtro de Kalman Estendido e do modelamento matemático dos sensores utilizados; 2

11 2. Projeto e desenvolvimento de hardware e firmware para um módulo central de comunicação e controle para a plataforma de Stewart; 3. Desenvolvimento de um protocolo de comunicação para controle e aquisição de dados dos atuadores; 4. Projeto e desenvolvimento de hardware e firmware para um módulo de IMU utilizada no sensoriamento da plataforma; 5. Desenvolvimento de um software em linguagem C++ para a aquisição de dados e processamento de imagens através do uso de bibliotecas do OpenCV (Open Source Computer Vision Library); 6. Validação dos modelos adotados através da comparação entre as respostas dos sensores com as simulações obtidas a partir da cinemática direta do deslocamento dos atuadores; 1.3 Estrutura Este trabalho é organizado da seguinte maneira: na Seção 2 são apresentados os principais conceitos matemáticos que possibilitam inferir a posição e orientação de um corpo rígido. Na Seção 3 é apresentada a metodologia proposta a partir de um diagrama mostrando integração entre todos os componentes envolvidos no trabalho, assim como o processo de validação dos modelos utilizados. Na Seção 4, a aplicação da metodologia é apresentada, bem como os principais resultados obtidos. Por fim, na Seção 5 são apresentadas as principais considerações e conclusões em relação ao trabalho proposto. 3

12 2 REFERENCIAL TEÓRICO Nesta seção são abordados os principais conceitos utilizados para a aplicação da metodologia proposta. Primeiramente, são definidas as equações necessárias para a descrição geométrica da plataforma de Stewart. A seguir, o Filtro de Kalman Estendido é apresentado como ferramenta para a filtragem e fusão de sensores, bem como o algoritmo necessário para sua execução, possibilitando a estimativa de posição e orientação em sistemas dinâmicos com seis graus de liberdade. Na sequência, é dada uma breve introdução à teoria dos quatérnios, assim como as principais equações matemáticas utilizadas para a representação da dinâmica de posição e orientação de um corpo rígido. Por fim, são demostrados os modelos matemáticos para o uso do giroscópio, do acelerômetro e da câmera, que são utilizados no passo de correção do Filtro de Kalman Estendido. 2.1 Plataforma de Stewart Uma plataforma de Stewart consiste em uma base fixa e uma móvel, conectadas por juntas através de seis hastes ou atuadores lineares, fornecendo ao sistema a capacidade de movimentação com seis graus de liberdade. Tal configuração foi proposta por Doug Stewart em 1965 para ser utilizada em simuladores de voo (STEWART, 1965). Este trabalho utiliza uma plataforma de Stewart como ambiente de simulação e validação, portanto é de particular interesse a descrição geométrica da plataforma. Figura 1 - Plataforma de Stewart: (a) estrutura geral, (b) relações geométricas das bases da plataforma. (a) (b) Fonte: Retirado de (DE FARIA, 2016). 4

13 Considere a plataforma de Stewart da Figura 1(a), onde são definidas as coordenadas de referência do sistema. O I é o sistema de coordenadas globais. O B e O T são sistemas de coordenadas locais para a base e topo da plataforma, respectivamente. O manipulador possui i = (1,,6) hastes onde cada junta b i e t i são vértices de um hexágono inscrito em um círculo de raio r B e r T. Como mostra a Figura 1(b), os ângulos φ B e φ T definem o tamanho das bordas da plataforma e espaço entre cada par de juntas (DE FARIA, 2016). As juntas b i e t i são geometricamente definidas por um vetor B i R 3 e T i R 3 para a base e o topo da plataforma, respectivamente, relativos aos seus respectivos sistemas de coordenadas locais e são dados por: onde: r B cos(λ i ) r T cos(ψ i ) B i = [ r B sin(λ i )], T i = [ r T sin(ψ i )], (2.1) 0 0 iπ λ i = { 3 φ B, 2 i = (1,3,5) λ i 1 φ B, i = (2,4,6) iπ ψ i = { 3 φ T, 2 i = (1,3,5) ψ i 1 φ T, i = (2,4,6). (2.2) 2.2 Filtro de Kalman Estendido Em diversas áreas da engenharia é recorrente o uso de sensores, contudo, os sinais medidos normalmente trazem erros agregados decorrentes de ruídos integrados à medição. O Filtro de Kalman é uma das técnicas mais conhecidas para a filtragem de sinais, pois fornece um método recursivo para estimação de estados na presença de ruído, cuja estimativa é uma função densidade de probabilidade Gaussiana com média μ e covariância Σ. No contexto de localização, a saída do Filtro de Kalman é uma distribuição de prováveis posições ao invés de uma única estimação (CHOSET, 2005). O Filtro de Kalman pode ser considerado uma aplicação direta dos filtros de Bayes para sistemas dinâmicos lineares, podendo ser separado em duas etapas distintas: predição e correção. Na primeira, a estimativa é dada através do modelo matemático do sistema e, posteriormente, a correção é realizada através dos dados provenientes de sensores. A incerteza resultante na saída do filtro é menor que a dada pelo modelo ou pelos sensores, minimizando a covariância do erro de estimação (THRUN, BURGARD e FOX, 2005). Por ser limitado a 5

14 sistemas dinâmicos lineares, uma extensão da teoria foi desenvolvida para sistemas discretos não-lineares, conhecida como Filtro de Kalman Estendido. Para estimar os estados, é necessário definir as equações que integram a dinâmica do sistema com os dados medidos pelos sensores. O filtro baseia-se no modelo em espaço de estados sujeito a ruído gaussiano aditivo: x k = g(u k, x k 1 ) + ε k, z k = h(x k ) + δ k, (2.3) onde k é a amostra do tempo discreto, o vetor x k R n representa os estados e o vetor u k R m a entrada do sistema. A variável z k R p representa o sinal medido pelos sensores. As incertezas do modelo são dadas por ε k, que possui média zero e covariância R k. O ruído δ k integrado a medição possui média zero e covariância Q k. O Filtro de Kalman Estendido processa a cada amostra temporal k a estimativa Gaussiana do vetor de estados x k, que possui média μ k e covariância Σ k. Para tratar o sistema da Equação (2.3), faz-se uso da aproximação de Taylor de primeira ordem, ou seja, aproximase a função não linear g( ) por: g(u k, x k 1 ) g(u k, μ k 1 ) + g(u k,μ k 1 ) (x μ k 1 μ k 1 k 1), =:G k (2.4) sendo G k a Matriz Jacobiana do modelo. A mesma aproximação pode ser feita para as nãolinearidades envolvendo os sensores: h(x k ) h( μ k) + h( μ k ) μ k =:H k (x k μ k). (2.5) A Tabela 1 apresenta o algoritmo resultante do Filtro de Kalman Estendido. Nas linhas 2 e 3 é realizada a predição dos estados e da covariância através do modelo do sistema. Posteriormente, nas linhas 4 até 6, o chamado Ganho de Kalman K k é calculado e a medição é incorporada através do sinal z k, corrigindo o valor estimado. É interessante notar que K k representa a relevância que o sinal medido através dos sensores terá na composição do sinal filtrado. Por fim, um novo valor de covariância Σ k é calculado para a próxima interação do filtro. 6

15 Tabela 1 - Algoritmo do Filtro de Kalman. 1: Algoritmo Filtro_Kalman_Extendido(μ k 1, Σ k 1, u k, z k ) 2: μ k = g(u k, μ k 1 ) 3: Σ k = G k Σ k 1 G k T + R k 4: K k = Σ k H k T (H k Σ k H k T + Q k ) 1 5: μ k = μ k + K k (z k h(μ k )) 6: Σ k = (I K k H k )Σ k 7: return μ k, Σ k Fonte: THRUN, BURGARD e FOX (2005). Para um melhor entendimento do funcionamento do Filtro de Kalman Estendido, a Figura 2 ilustra graficamente o funcionamento do filtro. A Figura 2(a) mostra um exemplo da representação das estimativas dos sensores e do modelo. Como os dados são representados por uma função densidade de probabilidade gaussiana, possuem média e covariância atribuídas. Como pode ser visto, o modelo fornece uma medida mais precisa, pois possui uma covariância (incerteza) menor que o resultado apresentado pelos sensores. A Figura 2(b) mostra a estimativa após a aplicação do Filtro de Kalman Estendido, onde o resultado possui uma covariância menor que a dada pelo modelo ou pelos sensores, garantindo uma incerteza menor após a aplicação do filtro. Figura 2 - Funcionamento do Filtro de Kalman Estendido. Fonte: O autor. 7

16 Em consequência de suas características, o Filtro de Kalman Estendido também pode ser utilizado para a fusão de sensores. Este trabalho tem particular interesse nessa funcionalidade, pois faz uso da visão computacional para corrigir problemas advindos da utilização de uma IMU, fornecendo uma estimativa mais precisa. 2.3 Quatérnios Quatérnios podem ser utilizados em diferentes áreas da matemática teórica e aplicada, possuindo particular uso para rotações tridimensionais, pois fornecem uma alternativa aos tradicionais ângulos de Euler, reduzindo o esforço computacional envolvido. Quatérnios podem ser definidos como uma extensão dos números complexos e foram originalmente desenvolvidos pelo matemático William Rowan Hamilton (HAMILTON, 1844). A história diz que enquanto Hamilton estava andando com sua esposa pelas ruas de Dublin, teria, após um insight, gravado nas pedras de uma ponte a equação que deu origem aos quatérnios (KUIPERS, 1999) e que atualmente é local de peregrinação de matemáticos do mundo inteiro. Um quatérnio pode ser definido da seguinte maneira: q = η + ε 1 i + ε 2 j + ε 3 k, (2.6) onde q H é um número hipercomplexo, η R é a parte real e ε = [ε 1 ε 2 ε 3 ] T R 3 é a parte imaginária do quatérnio. Também temos as componentes i, j e k que determinam a base padrão de ortogonalidade de R 3 e devem satisfazer a seguinte relação: i 2 = j 2 = k 2 = ijk = 1. (2.7) O quatérnio também pode ser representado em sua forma vetorial, que é dada por: q q = [ η ε ] R4. (2.8) Como já citado anteriormente, operações envolvendo quatérnios fornecem uma alternativa aos Ângulos de Euler para rotações no espaço tridimensional, por isso são largamente utilizados em diversas áreas, como, por exemplo, computação gráfica. Por conta de sua simplicidade, elegância matemática e falta de singularidades, também são muito populares para representação da posição e orientação de um corpo rígido (DIEBEL, 2006). Um quatérnio q r com tamanho unitário, ou seja, q r = 1, representado por: cos ( θ q r q r = [ 2 ) r sin ( θ ] (2.9) 2 ) pode ser utilizado para efetuar uma rotação de um ângulo θ R em torno do vetor r R 3. Utilizando a Equação (2.9), podemos definir a seguinte relação: 8

17 y = R(q r )x, (2.10) onde o vetor y R 3 é o resultado da rotação do vetor x R 3 por um ângulo θ em torno de r. Esta propriedade é resultado da multiplicação de R(q r ) R 3 3 pelo vetor x, onde R(q r ) é chamada Matriz de Rotação e pode ser definida por: R(q) = I 3 + 2ηS(ε) + 2S 2 (ε), (2.11) onde I 3 é uma matriz identidade de ordem três e o operador S( ) representa o produto cruzado e é dado pela Equação (2.12), descrita por: 0 ε 3 ε 2 S(ε) = [ ε 3 0 ε 1 ], ε = [ ε 2 ] R 3. (2.12) ε 2 ε 1 0 ε 3 Para melhor entendimento de uma operação de rotação envolvendo quatérnios, a Figura 3 representa a rotação do vetor x em torno do eixo de rotação r por um ângulo θ e pode ser vista abaixo: ε 1 Figura 3 Representação de rotação utilizando quatérnios. Fonte: Adaptado de (Underwater Navigation System, 2017). As Equações (2.13) e (2.14) representam, respectivamente, a equivalência dos Ângulos de Euler α, β e γ a partir de q e vice-versa, dadas por: α arctan ( 2(ηε 1+ε 2 ε 3 ) 1 2(ε 2 1 +ε 2 2 ) [ β] = arcsin(2(ηε 2 + ε 3 ε 1 )), (2.13) γ [ arctan ( 2(ηε 3+ε 1 ε 2 ) 1 2(ε 2 2 +ε 2 3 ) ] cos ( α 2 ) cos (β 2 ) cos (γ 2 ) sin (α 2 ) sin (β 2 ) sin (γ 2 ) q = sin ( α ) cos 2 (β) cos 2 (γ) cos 2 (α) sin 2 (β) sin 2 (γ) 2 cos ( α ) sin 2 (β) cos 2 (γ) sin 2 (α) cos 2 (β) sin 2 (γ). (2.14) 2 [ cos ( α ) cos 2 (β) sin 2 (γ) sin 2 (α) sin 2 (β) cos 2 (γ) 2 ] 9

18 Por fim, podemos representar as dinâmicas de posição e orientação de um corpo rígido baseado em quatérnios pela Equação (2.15), dada por: q ω [ ] = p v [ 1 [ ε T 2 ηi 3 + S(ε) ] ω α v a g ], (2.15) onde q R 4 representa a orientação em quatérnios, ω R 3 é o vetor de velocidade angular, α R 3 é o vetor de aceleração angular, p R 3 é o vetor de posição linear, v R 3 é o vetor de velocidade linear, a R 3 é o vetor de aceleração linear, g é o vetor de aceleração gravitacional (DE FARIA, 2016). No contexto desse trabalho, o modelo descrito pela Equação (2.15) é utilizado no Filtro de Kalman Estendido para descrever a dinâmica do topo da plataforma. 2.4 Modelos do Giroscópio e do Acelerômetro Um giroscópio é um dispositivo capaz de medir a velocidade angular e pode ser representado como um vetor ω G R 3, definido a partir de O S, que é o sistema de coordenadas locais do sensor, como pode ser visto na Figura 4. É interessante notar que o sensor pode estar orientado de forma distinta em relação ao sistema de interesse, portanto, é necessário obter um modelo que relacione a velocidade angular ω G medida pelo sensor com a velocidade angular ω da plataforma, que é referenciada em coordenadas globais O I. Tendo em vista as considerações abordadas e a teoria de quatérnios apresentada anteriormente, podemos definir as matrizes de rotação R(q G ) e R T (q), que podem ser utilizadas para relacionar ω com as medidas obtidas pelo sensor. Então, o modelo do giroscópio pode ser representado por: ω G = R(q G )R T (q)ω + b G + δ G, (2.16) onde R(q G ) descreve a orientação do sensor na plataforma, R T (q) é utilizada para transformar do sistema de coordenadas globais da plataforma para o sistema de coordenadas locais do sensor. Ainda temos b G R 3, que é offset do sensor e representa o valor constante da diferença entre o valor medido e o valor real e δ G representa o ruído do sensor, que possuí média zero e covariância Q G. 10

19 Figura 4 - Representação do sistema de coordenadas locais para o sensor. Fonte: O autor. Um acelerômetro, por outro lado, é um sensor capaz de medir as acelerações que atuam em seus eixos e pode ser representado como um vetor a A R 3, definido em O S, que é o sistema de coordenadas locais do sensor. O acelerômetro mede as acelerações relativas às translações do sistema de interesse juntamente com a aceleração da gravidade. Outro fator que deve ser levado em consideração para descrever um modelo para o sensor, é o fato de que quando instalado fora da origem do sistema e submetido a uma rotação, o acelerômetro também mede as forças inerciais decorrentes dessa dinâmica (STEVEN C, RICHARD P, et al., 2008), denominadas força de coriolis e força centrífuga, como ilustra a Figura 5. Figura 5 - Representação das forças inerciais. Fonte: Adaptado de (CORIOLIS FORCE, 2017). Com isso, podemos obter o modelo do acelerômetro, dado por: a A = R(q A )R T (q)((a + g) + H(ω, α)o A ) + b A + δ A, (2.17) onde R(q A ) e R T (q) são matrizes de rotação utilizadas para orientar as acelerações do sistema de interesse para o sistema de coordenadas locais do sensor e o A R 3 representa o offset do sensor em relação origem de O I. Os efeitos das forças inercias são dadas por (VANCIN, 2016): H(ω, α) = S(ω) 2 + S(α). (2.18) 11

20 Por fim, b A R 3 é offset de medição do sensor e δ A representa o ruído integrado na medição, que possuí média zero e covariância Q A. 2.5 Modelo da Câmera Como descrito anteriormente, o Filtro de Kalman Estendido efetua um passo de correção baseado em dados medidos por sensores para corrigir a estimativa fornecida pelo modelo. Este trabalho considera imagens de uma câmera para efetuar a correção, onde as relações entre pontos previamente conhecidos no espaço tridimensional levam a uma projeção nas imagens que estão relacionadas com a posição e orientação da câmera. Como este trabalho busca instrumentar uma câmera como sensor e fazer a validação de seu modelo, serão demostradas as equações matemáticas do processo de formação de imagem. Figura 6 - Representação da captura de um ponto por uma câmera. Fonte: Adaptado de (MARIOTTINI e PRATTICHIZZO, 2005). A Figura 6 mostra uma câmera com orientação q e posição p, onde um ponto ξ i R 3, referenciado nas coordenadas globais, tem sua projeção no plano da imagem dado por m i R 2. Baseado em (MARIOTTINI e PRATTICHIZZO, 2005), obtém-se a seguinte relação: Z c m i = K C R(q C )R T (q)(ξ i p), (2.19) onde m i = [m i 1] T é a posição em pixels da projeção do ponto ξ i no frame de imagem, R(q C ) e R T (q) são matrizes de rotação utilizadas para orientar a câmera em relação ao sistema de coordenadas globais e K C R 3 3 é a matriz com os parâmetros intrínsecos, dada por: 12

21 fs x fs θ o x K C = [ 0 fs y o y ]. (2.20) Os termos da matriz K C são parâmetros de calibração da câmera, onde (o x, o y ) é a coordenada em pixels do ponto principal no plano da imagem (intersecção entre o plano da imagem e o eixo z c ), s x e s y representam o número de pixels por unidade de distância nas coordenadas da imagem, f é a distância focal e fs θ representa a inclinação do pixel (MA, SOATTO, et al., 2012). Por fim, ξ i pode ser descrito pelas coordenadas da câmera, de acordo com a Figura 6, onde ξ i = [X c, Y c, Z c ] T. O parâmetro Z c normalmente é desconhecido, portanto, podemos simplificar a Equação (2.19) aplicando o operador de produto cruzado em ambos os lados da equação (VANCIN, 2016), pois S(m i)m i = 0, resultando em: 0 = S(m i)k C R(q C )R T (q)(ξ i p). (2.21) 13

22 3 METODOLOGIA Esta seção aborda, de maneira concisa, todos os métodos utilizados para a instrumentação e sensoriamento da plataforma de Stewart, que possibilitam estimar a posição e orientação do topo da plataforma através do Filtro de Kalman Estendido, bem como os procedimentos utilizados para a validação do sistema. Diagramas são mostrados, possibilitando ao leitor um melhor entendimento do processo como um todo. Primeiramente, é mostrada uma visão geral do processo de aquisição de dados, bem como a integração de todos os sensores envolvidos. Posteriormente, são mostrados os métodos utilizados para a validação dos modelos adotados. A Figura 7 mostra a renderização da plataforma de Stewart utilizada. Figura 7 - Plataforma de Stewart utilizada. Fonte: O autor. 3.1 Instrumentação e Aquisição de Dados Como já citado, o Filtro de Kalman Estendido utiliza, em seu passo de correção, dados de um acelerômetro, um giroscópio e uma câmera, portanto, foram desenvolvidos os hardwares e firmwares necessários para o sensoriamento e aquisição de dados da plataforma. O Diagrama 1 mostra a integração de todos os sensores. O módulo central faz o gerenciamento dos dados obtidos do módulo da IMU, localizado no topo da plataforma, há uma taxa de amostragem prédefinida e transmite para um computador, que efetua a aquisição e processamento da imagem dos marcadores, finalizando o processo com o armazenamento dos dados. O módulo central também faz a aquisição do deslocamento dos atuadores, que possibilita, através de cinemática direta, inferir com precisão a posição e orientação do topo da plataforma de Stewart, permitindo validar o sistema. 14

23 Diagrama 1 - Processo de aquisição de dados. Fonte: O autor. A plataforma de Stewart utilizada possui seis atuadores lineares movidos por motores DC com sensor encoder embutido, permitindo medir o deslocamento angular do eixo do motor e, consequentemente, o deslocamento linear do atuador. Cada atuador possui um módulo de controle próprio, que são conectados através de um barramento de comunicação RS-485 com o módulo central. Através de um protocolo de comunicação, o módulo central controla cada um dos atuadores e faz a aquisição de dados dos encoders, que são utilizados para efetuar a cinemática direta para posterior validação do sistema. Através de um programa em linguagem C++, os dados são recebidos e armazenados em um computador, possibilitando também um sincronismo com a aquisição das imagens. O processamento de imagens é realizado com o uso de funções do conjunto de bibliotecas do OpenCV. A partir dos dados da câmera, são obtidos as posições m i de cada um dos marcadores no frame de imagem, que também são utilizados no processo de correção do Filtro de Kalman Estendido. 3.2 Validação dos Modelos Este trabalho busca, além de efetuar a instrumentação e sensoriamento da plataforma, validar os modelos utilizados através dos dados de deslocamento dos atuadores lineares. Essa validação é de grande relevância, pois dessa forma é possível garantir uma descrição fiel dos modelos utilizados em relação ao sistema real, possibilitando a aplicação do algoritmo do Filtro de Kalman Estendido. O Diagrama 2 ilustra o processo de validação, onde os sinais provenientes dos módulos dos atuadores são tratados, possibilitando a simulação dos sensores e a comparação com os valores medidos da IMU e das posições dos marcadores, obtidas pelo processamento de imagens da câmera. 15

24 Diagrama 2 - Processo de validação dos modelos utilizados. p, q ω, α v, a Fonte: O autor. A cinemática direta é um processo que busca estimar posição p e orientação q do topo da plataforma a partir do valor de tamanho dos atuadores lineares. Embora também seja uma técnica que possibilite estimar posição e orientação, sua implementação é difícil, pois para um mesmo conjunto de dados de tamanho dos atuadores lineares, são múltiplas soluções possíveis para posição e orientação do topo. Tendo em vista as considerações abordadas, a cinemática direta leva a algoritmos lentos e que demandam de grande esforço computacional, contudo, é um excelente mecanismo para validação, tanto dos modelos, como do próprio Filtro de Kalman Estendido. A partir dos valores de p e q podemos estimar também todas as outras variáveis que definem a dinâmica da plataforma, como mostra a Equação (2.15). Assim, os modelos das Equações (2.16), (2.17) e (2.19) que descrevem o giroscópio, o acelerômetro e a câmera, respectivamente, podem ser utilizados para simular os sensores. Por fim, os dados dos sensores simulados podem ser comparados com os dados medidos, possibilitando validar os modelos e garantir a descrição matemática de todo o sistema. 3.3 Filtro de Kalman Estendido A dedução das equações e a aplicação do Filtro de Kalman Estendido não fazem parte do escopo desse trabalho, porém tanto os modelos como os resultados obtidos em conjunto com os pesquisadores do GACS serão apresentados, objetivando mostrar a eficiência dos modelos. 16

25 O Diagrama 3 ilustra o processo de estimativa e validação. O filtro estima posição e orientação através da fusão dos dados da IMU e das posições dos marcadores. Como já foi demostrado, essa mesma estimativa é viável através da cinemática direta, que embora resulte em um processo lento e que demanda de um grande esforço computacional, possibilita validar o os resultados obtidos através da comparação entre as estimativas de ambos os processos utilizados. Diagrama 3 - Estimativa de posição e orientação através do Filtro de Kalman Estendido. p, q erro p, q Fonte: O autor. 17

26 4 APLICAÇÃO E RESULTADOS Nesta seção são detalhados todos os procedimentos adotados para a aplicação da metodologia proposta, bem como os resultados obtidos ao longo do desenvolvimento deste trabalho. Primeiramente, são demonstrados todos métodos utilizados para a aquisição de dados dos atuadores e da IMU, bem como as principais considerações relativas aos hardwares desenvolvidos. Também são demonstrados os procedimentos adotados para aquisição e processamento das imagens, utilizadas para a obtenção da posição dos marcadores. O sistema é validado através da base teórica apresentada previamente utilizando a cinemática direta do deslocamento dos atuadores da plataforma de Stewart, onde os modelos adotados são simulados e comparados com os dados medidos. 4.1 Instrumentação e Aquisição de Dados Módulo Central Como descrito pelo Diagrama 1, desenvolveu-se um módulo central de processamento responsável por efetuar a comunicação e aquisição de dados dos módulos de controle dos atuadores e da IMU. O módulo central também é responsável por fazer a comunicação com um computador, onde os dados são armazenados. Os principais componentes utilizados podem ser vistos na Tabela 2. Tabela 2 - Componentes utilizados para o módulo central. Tipo Part Number Empresa Microcontrolador STM32F072RBT6 STMicroelectronics Transceptor ST485E STMicroelectronics Regulador Chaveado MP158EN Monolithic Power Systems Regulador Linear TLV2217 Texas Instruments Fonte: O autor. Para a escolha do microcontrolador, levou-se em consideração a necessidade de uma comunicação com altas taxas de transmissão de dados. O microcontrolador utilizado possui um núcleo ARM Cortex-M0 de 32 bits com um módulo USB integrado, tornando a comunicação com computador rápida e eficiente. Ele também pode operar em uma frequência de até 48 MHz, com 128 Kbytes de memória flash e 16 Kbytes de memória SRAM, configurando ao módulo alta capacidade de processamento. Por fim, o microcontrolador também possui suporte em 18

27 hardware para controle de fluxo de sinal para transceptores RS-485, que é a interface de comunicação com os outros módulos do sistema. Para o projeto e desenvolvimento da placa de circuito impresso, utilizou-se o software Altium Designer, da Empresa Altium. O esquemático completo do circuito pode ser visualizado no Apêndice C e o resultado pode ser visto renderizado na Figura 8. Figura 8 - Placa de circuito impresso do módulo central: (a) top layer, (b) bottom layer, (c) renderização do top, (d) renderização do bottom. (a) (b) (c) (d) Fonte: O autor. 19

28 Por fim, o desenvolvimento de um projeto com média e alta complexidade demanda de ferramentas de suporte que permitam acelerar seu desenvolvimento. Tendo em vista esse contexto, outro fator relevante para a escolha do microcontrolador adotado é seu suporte a serial wire debug (SWD), que acelera significativamente o desenvolvimento de qualquer firmware. Nesse projeto foi utilizado o software µvision IDE, da empresa Keil, tanto para desenvolvimento do firmware para o módulo central, como para o módulo da IMU, que será detalhando adiante Atuadores A medição do deslocamento linear dos atuadores é de grande relevância para o trabalho proposto, pois através de cinemática direta é possível obter a posição e orientação do topo da plataforma para validar os modelos adotados e os resultados encontrados com a aplicação do Filtro de Kalman Estendido. A plataforma de Stewart utilizada possui controladores próprios para cada atuador, que já haviam sido desenvolvidos, conectados através de uma rede RS-485, como mostra a Figura 9. Cada módulo pode retornar a posição dos atuadores baseado na leitura dos encoders presentes em cada um dos motores DC. Figura 9 - Rede RS-485 para aquisição e controle dos atuadores. Fonte: O autor. RS-485 é um padrão de hardware utilizado para transmissão de informação a longas distâncias em ambientes ruidosos. A principal característica dessa topologia de implementação é o fato de que a informação é transmitida através de um par diferencial, onde o estado lógico se dá pela diferença de potencial, positiva ou negativa. Como consequência, a RS-485 se torna uma excelente escolha para ser utilizada em ambientes com altos níveis de ruído, pois ele é induzindo no meio de transmissão, não alterando a diferença de tensão no par. Para o controle e aquisição dos dados de todos os módulos dos atuadores, desenvolveuse um protocolo de comunicação próprio para a plataforma de Stewart, que pode ser utilizado 20

29 em conjunto com o módulo central para trabalhos futuros, como, por exemplo, efetuar o controle da plataforma. A Figura 10 mostra o frame de comunicação, onde cada módulo escravo pode acessar e ser acessado pelo mestre através de um endereço próprio. Com o protocolo desenvolvido, é possível controlar cada atuador separadamente e fazer a aquisição do valor dos encoders. Em decorrência do alto nível de ruído proveniente dos motores DC, utilizou-se o método de detecção de erros conhecido como CRC (Cyclic Redundancy Check), garantindo robustez e confiabilidade para a comunicação. Outro fator a ser levado em consideração é que o protocolo possui um pacote de dados variável, permitindo uma troca de dados bastante flexível. Figura 10 - Protocolo de comunicação. Fonte: O autor Sensores Inerciais Uma unidade de sensores inerciais é um dispositivo eletrônico capaz de medir aceleração linear e velocidade angular através do uso de acelerômetros e giroscópios. Tais dispositivos fornecem informações valiosas para descrever a dinâmica de um corpo rígido. Integrando a aceleração linear podemos inferir a velocidade linear de um corpo e de forma similar, integrando a velocidade linear podemos determinar sua posição linear. O mesmo pode ser feito para o giroscópio, onde a posição angular também pode ser inferida com a integração da velocidade angular. Pelos motivos citados, esses sensores são largamente utilizados em diversas aplicações, como, por exemplo, estimar os ângulos necessários para o controle de um veículo aéreo não tripulado. Para uma correta estimativa, o processo de integração dos dados provenientes desses sensores inerciais requer medidas com alta precisão, pois erros numéricos são integrados e levam ao processo conhecido como drift. Por exemplo, sensores comerciais comuns possuem desvios em relação ao valor real de medição, conhecidos como offset. A contínua integração desse offset faz o sinal se deslocar continuamente em relação ao valor esperado. Portando, 21

30 embora sensores inercias sejam uma boa alternativa para diversas aplicações, constantemente necessitam da medição de outros sensores para uma correta estimativa. Outra grande fonte de problemas envolvendo unidades inerciais é a escolha de sensores pouco precisos e que retornam medições com altos níveis de ruído. Para o desenvolvimento desse trabalho, optou-se pelo sensor LSM6DS3H, da empresa STMicroelectronics, que possui um acelerômetro e um giroscópio, cada um com medição nos 3 eixos. Para uma escolha apropriada, levou-se em consideração as capacidades de medição de ambos os sensores. O acelerômetro possui fundo de escala de até ±16 g, ou seja, mede acelerações de até 16 vezes a aceleração da gravidade, enquanto o giroscópio possui fundo de escala de ±2000 dps. Outro fator determinante para a escolha do sensor é sua taxa de amostragem e ambos os sensores trabalham com altas taxas de até 3,33 khz. Os principais componentes utilizados no projeto do módulo para instrumentação da IMU podem ser vistos na Tabela 3. Tabela 3 - Componentes utilizados para o módulo da IMU. Tipo Part Number Empresa Sensor LSM6DS3H STMicroelectronics Microcontrolador STM32F030F4P6 STMicroelectronics Transceptor ST485E STMicroelectronics Regulador Chaveado MP158EN Monolithic Power Systems Regulador Linear TLV2217 Texas Instruments Fonte: O autor. O microcontrolador utilizado possui um núcleo ARM Cortex-M0 de 32 bits e pode operar em uma frequência de até 48 MHz, com 14 Kbytes de memória flash e 4 Kbytes de memória SRAM. A comunicação com o módulo central é feita através de comunicação serial RS-485, portanto, da mesma forma que o módulo central, esse microcontrolador também possui suporte em hardware para controle de fluxo de sinal para transceptores RS-485. Para o projeto e desenvolvimento da placa de circuito impresso, utilizou-se o software Altium Designer. O esquemático completo pode ser visto no Apêndice C e o resultado final da placa de circuito impresso pode ser visto na Figura

31 Figura 11 - Placa de circuito impresso do módulo da IMU: (a) top layer, (b) bottom layer, (c) renderização do top, (d) renderização do bottom. (a) (b) (c) (d) Fonte: O autor Processamento de Imagem e Aquisição dos Marcadores A visão computacional é uma área há muito explorada na engenharia, contudo, a aquisição, processamento e análise de imagens são processos complexos e demandam de grande esforço computacional. O OpenCV é um conjunto de bibliotecas open source voltado para processamento de imagens, possuindo mais de 2500 algoritmos otimizados que permitem abstrair o baixo nível, acelerando significativamente o desenvolvimento de um projeto. Esses algoritmos podem ser usados para reconhecimento de faces e ações humanas, identificação de objetos, extração de modelos 3D, aplicação de filtros, etc. Por suportar diversas linguagens de 23

32 programação, como C++, Python e MATLAB, se tornou a escolha mais adequada para o presente trabalho. Tendo em vista a aquisição de imagens com alta qualidade, utilizou-se uma webcam Logitech C270, com resolução HD, posicionada no topo da plataforma, como mostra a Figura 12(a). Os marcadores foram posicionados na base da plataforma e podem ser vistos na Figura 12(b). Como vemos, três marcadores foram posicionados no mesmo plano e um deles em um plano diferente, pois dessa forma a posição de um marcador em relação ao outro na imagem resulta em diferenças mais significativas a medida que a plataforma se move, sendo assim, uma estimativa mais precisa sobre a posição e orientação do topo da plataforma pode ser obtida. Figura 12 Processamento de imagem: (a) posicionamento da câmera, (b) posicionamento dos marcadores. (a) (b) Fonte: O autor. Para o correto funcionamento do Filtro de Kalman Estendido, a sincronização da aquisição da IMU com a captura da imagem dos marcadores é de vital importância. Para garantir a sincronização, utilizou-se um programa em linguagem C++, desenvolvido no software Visual Studio, da empresa Microsoft, onde os dados são obtidos do módulo central, a uma taxa de amostragem específica, via comunicação serial, dando início ao processo de aquisição e processamento de imagem. O processo pode ser visualizado no Fluxograma 1, onde são mostrados os principais procedimentos para a aquisição de um marcador e as principais funções utilizadas encontram-se listadas e detalhadas no Apêndice A. Após a captura, a imagem deve ser convertida do padrão RGB (do inglês Red, Green e Blue) para HSV (do inglês Hue, Saturation e Value). O padrão RGB é largamente adotado para captura e a armazenamento de imagens, pois através da combinação de três cores primárias (vermelho, verde e azul) em diferentes intensidades é possível representar todas as outras cores 24

33 que um olho humano é capaz de perceber. Contudo, a representação das cores no modelo RGB não é intuitiva, portanto, a computação gráfica muitas vezes adota o padrão HSV, onde as cores são representadas em função de sua tonalidade, saturação e brilho, se assemelhando mais com percepção humana para representação do espectro de cores. Fluxograma 1 - Processo de aquisição dos marcadores. Fonte: O autor. Após a conversão, o marcador desejado pode ser obtido através da escolha de uma faixa de tonalidade, saturação e brilho permitida. Para ilustrar esse processo, considere as imagens da Figura 13, onde a imagem capturada da Figura 13(a) é convertida para a imagem binária da Figura 13(b) de acordo com a faixa de interesse escolhida, ou seja, de acordo com o marcador desejado, que neste caso é o marcador de cor azul. Após esse procedimento, a detecção de contorno pode ser realizada, como mostra Figura 13(c). Se a imagem possuir contornos, ou seja, se um marcador está no campo de visão da câmera, é possível então obter a coordenada do marcador através das propriedades de momentos invariantes da imagem. Da mecânica fundamental, o momento de inercia de área descreve matematicamente a distribuição de matéria em torno de um eixo ou ponto arbitrário. De forma semelhante, o plano de irradiação de uma imagem binária, denotado por f(x, y), pode ser descrito em função de seus momentos invariantes (TEAGUE, 1980), onde: M 00 = x y f(x, y), (3.01) 25

34 é o momento de ordem zero e representa a área da imagem. Os momentos de primeira ordem são dados por: M 10 = x y xf(x, y), (3.02) M 01 = x y yf(x, y). (3.03) Com isso, podemos encontrar o centroide da imagem (x, y ), onde x = M 10 /M 00 e y = M 01 /M 00, que no contexto desse trabalho é coordenada do marcador. A Figura 13(d) mostra o resultado do processo descrito, onde a coordenada m i do marcador é localizada, finalizando o processamento da imagem. Por fim, os dados dos encoders, da IMU e das coordenadas de todos os marcadores são armazenados para serem utilizados na aplicação do Filtro de Kalman Estendido e na validação dos modelos utilizados. Figura 13 - Processamento de imagem através do OpenCV: (a) imagem capturada, (b) threshold da imagem, (c) detecção de contorno, (d) posição do marcador. (a) (b) m i (c) (d) Fonte: O autor. 26

35 4.2 Validação dos Modelos Estimativa da Posição e Orientação através da Cinemática Direta A análise cinemática da plataforma de Stewart consiste no estudo da relação entre a posição e orientação do topo da plataforma com o valor de tamanho ou deslocamento dos atuadores lineares. Essa relação é classificada como inversa ou direta. A primeira consiste em encontrar uma solução para o deslocamento dos atuadores em função da posição e orientação desejadas para o topo da plataforma, enquanto a segunda busca encontrar uma relação para posição e orientação do topo a partir do deslocamento dos atuadores. A cinemática inversa pode ser solucionada a partir de uma abordagem da geometria da estrutura e leva a um único resultado, contudo, a cinemática direta requer uma abordagem diferente, levando a múltiplas soluções. Em (CARDONA, 2015), a plataforma é modelada em função das restrições de movimento para cada uma das partes móveis através de uma série de equações que definem essas restrições e então métodos numéricos são utilizados para encontrar a solução para a cinemática direta. Este trabalho considera o uso de métodos matemáticos para minimizar uma função de custo J do erro do valor medido em relação ao valor estimado para o tamanho dos atuadores. O Fluxograma 2 ilustra o processo de minimização para uma determinada amostra k do valor medido L medido de tamanho dos atuadores com i = (1,,6). i Fluxograma 2 - Processo de minimização da função de custo através da função fminsearch do MATLAB. p x, p y, p z, α, β, γ B P, S P, L estimado L medido L estimado arg min J < Ε posição = [p x, p y, p z ] orientação = [α, β, γ] Fonte: O autor. Para realizar a minimização, utilizou-se a função fminsearch do software MATLAB, que pode ser utilizada para encontrar um valor mínimo para uma função multivariável a partir 27

36 de um valor inicial. Neste contexto, valores iniciais de posição (p x, p y e p z ) e orientação (α, β e γ) são utilizados para estimar o valor dos atuadores. Para isso, são utilizadas as Equações (2.1) e (2.2) para encontrar os vetores B i e T i que definem as juntas da plataforma em seus respectivos sistemas de coordenadas locais. Então, a teoria de quatérnios pode ser utilizada para rotacionar e transladar cada uma das juntas para o sistema de coordenadas globais: B P i = B ir(q B ) + [p x p y 0] T, (4.01) T P i = T ir(q T ) + [p x p y p z ] T, onde q B e q T são quatérnios utilizados para rotacionar cada uma das juntas da plataforma e podem ser obtidos a partir da Equação (2.14), que relaciona os ângulos de Euler com o quatérnio equivalente. É interessante notar que os ângulos α, β e γ são utilizados para rotacionar apenas o topo da plataforma, pois a base permanece fixa ao longo do tempo. A Figura 14 mostra o resultado da simulação para a plataforma em seu estado inicial. Figura 14 - Simulação da plataforma de Stewart, onde p x = 0. 70, p y = 0. 50, p z = 0. 58, α = 0, β = 0 e γ = 0. Fonte: O autor. Dessa forma, podemos encontrar o valor estimado de tamanho dos atuadores, onde: L estimadoi = (B P i T Pi )T (B P i T Pi ). (4.02) Com isso, temos: 28

37 6 J = (L medidoi L estimado i )2. (4.03) i=1 O algoritmo da função fminsearch estima a orientação e posição a partir de interações, onde os valores de p x, p y, p z, α, β e γ são modificados até a condição dada pelo Fluxograma 2 ser estabelecida, onde Ε é o mínimo valor admitido para J. Por fim, os dados de posição e orientação estimados são armazenados, sendo que para o último é necessário efetuar uma conversão dos ângulos de Euler encontrados para quatérnios, que serão utilizados para a simulação dos sensores. Por fim, para a simulação dos sensores, a plataforma foi programada para efetuar movimentos que possibilitassem uma dinâmica capaz de estimular todos os sensores. O resultado do deslocamento dos atuadores e da estimativa para a posição e orientação do topo da plataforma dada pela cinemática direta pode ser visto na Figura 15. Figura 15 - Cinemática direta: (acima) deslocamento dos atuadores ao longo do tempo, (inferior esquerda) estimativa da posição do topo, (inferior direita) estimativa da orientação do topo. Fonte: O autor. 29

38 4.2.2 Simulação dos Sensores Para simular os sensores, todos os processos descritos anteriormente foram considerados e o pós-processamento dos dados foi realizado no software MATLAB. Para uma melhor compreensão, as etapas executadas podem ser separadas da seguinte maneira: 1. Programação da plataforma através do módulo central para execução de movimentos capazes de excitar todos os sensores; 2. Aquisição dos dados de deslocamento dos atuadores e da IMU a taxas de amostragem F act e F imu, respectivamente; 3. Aquisição da posição dos marcadores através do processamento de imagens dos dados da câmera a uma taxa de amostragem F cam ; 5. Armazenamento dos dados de todos os sensores; 6. Obtenção da posição estimada p e orientação estimada q através da cinemática direta dos dados de deslocamento dos atuadores; 7. Obtenção da velocidade angular estimada ω, aceleração angular estimada α, velocidade linear estimada v e aceleração linear estimada a através da Equação (2.15); 8. Simulação dos sensores com o uso das Equações (2.16), (2.17) e (2.19); Em decorrência do fato que a câmera utilizada possui limitação de apenas 24 quadros por segundo, sua taxa de amostragem foi reduzida em relação a taxa de amostragem dos outros sensores para comportar essa limitação. Os valores utilizados podem ser vistos na Tabela 4. Tabela 4 - Taxas de amostragem utilizadas. Tipo Símbolo Valor [Hz] Atuadores F act 104 IMU F imu 104 Câmera F cam 104/5 Fonte: O autor. Por fim, as Figuras 16, 17 e 18 mostram os resultados da simulação dos sensores e a comparação com os valores medidos. Como pode ser visto, os sensores giroscópio e acelerômetro possuem uma dinâmica com alto grau de similaridade, diferindo apenas no nível de ruído agregado. A câmera também apresenta alto grau de similaridade, porém com baixo nível de ruído, pois é uma característica desse sensor. A análise dos resultados mostra que que os modelos representam uma descrição fiel dos sensores, provando que o ambiente de simulação condiz matematicamente com o sistema real. 30