Embed Size (px)
Citation preview
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 2017.
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: 90619-900 - Porto Alegre - RS - Brasil
Email: [email protected]
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 226 - CEP: 90619-900 - Porto Alegre
- RS- Brasil
Email: [email protected]
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.
Toda a nossa ciência, comparada com a realidade, é primitiva e
infantil - e, no entanto, é a coisa mais preciosa que temos.
Albert Einstein (1879 - 1955)
SUMÁRIO
1 INTRODUÇÃO ....................................................................................................... 1
1.1 Motivações ........................................................................................................ 2
1.2 Objetivos ........................................................................................................... 2
1.3 Estrutura ............................................................................................................ 3
2 REFERENCIAL TEÓRICO .................................................................................... 4
2.1 Plataforma de Stewart ....................................................................................... 4
2.2 Filtro de Kalman Estendido .............................................................................. 5
2.3 Quatérnios ......................................................................................................... 8
2.4 Modelos do Giroscópio e do Acelerômetro .................................................... 10
2.5 Modelo da Câmera .......................................................................................... 12
3 METODOLOGIA .................................................................................................. 14
3.1 Instrumentação e Aquisição de Dados ............................................................ 14
3.2 Validação dos Modelos ................................................................................... 15
3.3 Filtro de Kalman Estendido ............................................................................ 16
4 APLICAÇÃO E RESULTADOS........................................................................... 18
4.1 Instrumentação e Aquisição de Dados ............................................................ 18
4.1.1 Módulo Central ......................................................................................... 18
4.1.2 Atuadores .................................................................................................. 20
4.1.3 Sensores Inerciais ...................................................................................... 21
4.1.4 Processamento de Imagem e Aquisição dos Marcadores ......................... 23
4.2 Validação dos Modelos ................................................................................... 27
4.2.1 Estimativa da Posição e Orientação através da Cinemática Direta ........... 27
4.2.2 Simulação dos Sensores ............................................................................ 30
4.3 Filtro de Kalman Estendido ............................................................................ 32
5 CONCLUSÃO ....................................................................................................... 34
6 APÊNCIDES .......................................................................................................... 35
6.1 Apêndice A ..................................................................................................... 35
6.2 Apêndice B ..................................................................................................... 36
6.3 Apêndice C ..................................................................................................... 38
7 REFERÊNCIAS .................................................................................................... 42
1
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.
2
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;
3
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.
4
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).
5
Considere a plataforma de Stewart da Figura 1(a), onde são definidas as coordenadas de
referência do sistema. 𝑂𝐼 é o sistema de coordenadas globais. 𝑂𝐵 e 𝑂𝑇 são sistemas de
coordenadas locais para a base e topo da plataforma, respectivamente. O manipulador possui
𝑖 = (1,… ,6) hastes onde cada junta 𝑏𝑖 e 𝑡𝑖 são vértices de um hexágono inscrito em um círculo
de raio 𝑟𝐵 e 𝑟𝑇. Como mostra a Figura 1(b), os ângulos 𝜑𝐵 e 𝜑𝑇 definem o tamanho das bordas
da plataforma e espaço entre cada par de juntas (DE FARIA, 2016).
As juntas 𝑏𝑖 e 𝑡𝑖 são geometricamente definidas por um vetor 𝐵𝑖 ∈ ℝ3 e 𝑇𝑖 ∈ ℝ
3 para a
base e o topo da plataforma, respectivamente, relativos aos seus respectivos sistemas de
coordenadas locais e são dados por:
𝐵𝑖 = [𝑟𝐵 cos(𝜆𝑖)
𝑟𝐵 sin(𝜆𝑖)0
], 𝑇𝑖 = [𝑟𝑇 cos(𝜓𝑖)
𝑟𝑇 sin(𝜓𝑖)0
], (2.1)
onde:
𝜆𝑖 = {
𝑖𝜋
3−𝜑𝐵2, 𝑖 = (1,3,5)
𝜆𝑖−1 − 𝜑𝐵, 𝑖 = (2,4,6)
𝜓𝑖 = {
𝑖𝜋
3−𝜑𝑇2, 𝑖 = (1,3,5)
𝜓𝑖−1 − 𝜑𝑇 , 𝑖 = (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
6
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:
𝑥𝑘 = 𝑔(𝑢𝑘, 𝑥𝑘−1) + 𝜖𝑘,
𝑧𝑘 = ℎ(𝑥𝑘) + 𝛿𝑘, (2.3)
onde 𝑘 é a amostra do tempo discreto, o vetor 𝑥𝑘 ∈ ℝ𝑛 representa os estados e o vetor 𝑢𝑘 ∈ ℝ
𝑚
a entrada do sistema. A variável 𝑧𝑘 ∈ ℝ𝑝 representa o sinal medido pelos sensores. As
incertezas do modelo são dadas por 𝜖𝑘, que possui média zero e covariância 𝑅𝑘. O ruído 𝛿𝑘
integrado a medição possui média zero e covariância 𝑄𝑘.
O Filtro de Kalman Estendido processa a cada amostra temporal 𝑘 a estimativa
Gaussiana do vetor de estados 𝑥𝑘, que possui média 𝜇𝑘 e covariância Σ𝑘. Para tratar o sistema
da Equação (2.3), faz-se uso da aproximação de Taylor de primeira ordem, ou seja, aproxima-
se a função não linear 𝑔(∙) por:
𝑔(𝑢𝑘, 𝑥𝑘−1) ≈ 𝑔(𝑢𝑘, 𝜇𝑘−1) +
𝜕𝑔(𝑢𝑘,𝜇𝑘−1)
𝜕𝜇𝑘−1⏟ =:𝐺𝑘
(𝑥𝑘−1 − 𝜇𝑘−1), (2.4)
sendo 𝐺𝑘 a Matriz Jacobiana do modelo. A mesma aproximação pode ser feita para as não-
linearidades envolvendo os sensores:
ℎ(𝑥𝑘) ≈ ℎ( �̅�𝑘) +
𝜕ℎ( �̅�𝑘)
𝜕�̅�𝑘⏟ =:𝐻𝑘
(𝑥𝑘 − �̅�𝑘). (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 𝐾𝑘 é calculado e a medição é
incorporada através do sinal 𝑧𝑘, corrigindo o valor estimado. É interessante notar que 𝐾𝑘
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 Σ𝑘 é calculado para a próxima interação do
filtro.
7
Tabela 1 - Algoritmo do Filtro de Kalman.
1: 𝐀𝐥𝐠𝐨𝐫𝐢𝐭𝐦𝐨 𝐅𝐢𝐥𝐭𝐫𝐨_𝐊𝐚𝐥𝐦𝐚𝐧_𝐄𝐱𝐭𝐞𝐧𝐝𝐢𝐝𝐨(𝜇𝑘−1, Σ𝑘−1, 𝑢𝑘, 𝑧𝑘)
2: 𝜇𝑘= 𝑔(𝑢𝑘, 𝜇𝑘−1)
3: Σ𝑘 = 𝐺𝑘Σ𝑘−1𝐺𝑘𝑇 + 𝑅𝑘
4: 𝐾𝑘 = Σ𝑘𝐻𝑘𝑇(𝐻𝑘Σ𝑘𝐻𝑘
𝑇 + 𝑄𝑘)−1
5: 𝜇𝑘 = 𝜇𝑘 + 𝐾𝑘(𝑧𝑘 − ℎ(𝜇𝑘))
6: Σ𝑘 = (𝐼 − 𝐾𝑘𝐻𝑘)Σ𝑘
7: return 𝜇𝑘, Σ𝑘
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.
8
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:
𝒒 = 𝜂 + ε1𝑖 + ε2𝑗 + ε3𝑘, (2.6)
onde 𝒒 ∈ ℍ é um número hipercomplexo, 𝜂 ∈ ℝ é a parte real e 휀 = [ε1 ε2 ε3]𝑇 ∈ ℝ3 é a parte
imaginária do quatérnio. Também temos as componentes 𝑖, 𝑗 e 𝑘 que determinam a base padrão
de ortogonalidade de ℝ3 e devem satisfazer a seguinte relação:
𝑖2 = 𝑗2 = 𝑘2 = 𝑖𝑗𝑘 = −1. (2.7)
O quatérnio também pode ser representado em sua forma vetorial, que é dada por:
𝒒 → 𝑞 = [𝜂휀] ∈ ℝ4. (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 𝒒𝒓 com tamanho unitário, ou seja, ‖𝒒𝒓‖ = 1, representado por:
𝒒𝒓 → 𝑞𝑟 = [cos (
𝜃
2)
r sin (𝜃
2)
] (2.9)
pode ser utilizado para efetuar uma rotação de um ângulo 𝜃 ∈ ℝ em torno do vetor 𝑟 ∈ ℝ3.
Utilizando a Equação (2.9), podemos definir a seguinte relação:
9
𝑦 = 𝑅(𝑞𝑟)𝑥, (2.10)
onde o vetor 𝑦 ∈ ℝ3 é o resultado da rotação do vetor 𝑥 ∈ ℝ3 por um ângulo 𝜃 em torno de 𝑟.
Esta propriedade é resultado da multiplicação de 𝑅(𝑞𝑟) ∈ ℝ3×3 pelo vetor 𝑥, onde 𝑅(𝑞𝑟) é
chamada Matriz de Rotação e pode ser definida por:
𝑅(𝑞) = 𝐼3 + 2𝜂𝑆(휀) + 2𝑆2(휀), (2.11)
onde 𝐼3 é uma matriz identidade de ordem três e o operador 𝑆(∙) representa o produto cruzado
e é dado pela Equação (2.12), descrita por:
𝑆(휀) = [0 −휀3 휀2휀3 0 −휀1−휀2 휀1 0
] , ∀ 휀 = [
휀1휀2휀3] ∈ ℝ3. (2.12)
Para melhor entendimento de uma operação de rotação envolvendo quatérnios, a Figura
3 representa a rotação do vetor 𝑥 em torno do eixo de rotação 𝑟 por um ângulo 𝜃 e pode ser
vista abaixo:
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 𝑞 e vice-versa, dadas por:
[
𝛼𝛽𝛾] =
[ arctan (
2(𝜂𝜀1+𝜀2𝜀3 )
1−2(𝜀12+𝜀2
2))
arcsin(2(𝜂휀2 + 휀3휀1 ))
arctan (2(𝜂𝜀3+𝜀1𝜀2 )
1−2(𝜀22+𝜀3
2))]
, (2.13)
𝑞 =
[ cos (
𝛼
2) cos (
𝛽
2) cos (
𝛾
2) − sin (
𝛼
2) sin (
𝛽
2) sin (
𝛾
2)
sin (𝛼
2) cos (
𝛽
2) cos (
𝛾
2) − cos (
𝛼
2) sin (
𝛽
2) sin (
𝛾
2)
cos (𝛼
2) sin (
𝛽
2) cos (
𝛾
2) − sin (
𝛼
2) cos (
𝛽
2) sin (
𝛾
2)
cos (𝛼
2) cos (
𝛽
2) sin (
𝛾
2) − sin (
𝛼
2) sin (
𝛽
2) cos (
𝛾
2)]
. (2.14)
10
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:
[
�̇��̇��̇��̇�
] =
[ 1
2[
−휀𝑇
𝜂𝐼3 + 𝑆(휀)]𝜔
𝛼𝑣
𝑎 − 𝑔 ]
, (2.15)
onde 𝑞 ∈ ℝ4 representa a orientação em quatérnios, 𝜔 ∈ ℝ3 é o vetor de velocidade angular,
𝛼 ∈ ℝ3 é o vetor de aceleração angular, 𝑝 ∈ ℝ3 é o vetor de posição linear, 𝑣 ∈ ℝ3 é o vetor
de velocidade linear, 𝑎 ∈ ℝ3 é o vetor de aceleração linear, 𝑔 é 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 𝜔𝐺 ∈ ℝ3, definido a partir de 𝑂𝑆, 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 𝜔𝐺 medida pelo sensor com a velocidade angular 𝜔
da plataforma, que é referenciada em coordenadas globais 𝑂𝐼. Tendo em vista as considerações
abordadas e a teoria de quatérnios apresentada anteriormente, podemos definir as matrizes de
rotação 𝑅(𝑞𝐺) e 𝑅𝑇(𝑞), que podem ser utilizadas para relacionar 𝜔 com as medidas obtidas
pelo sensor. Então, o modelo do giroscópio pode ser representado por:
𝜔𝐺 = 𝑅(𝑞𝐺)𝑅𝑇(𝑞)𝜔 + 𝑏𝐺 + 𝛿𝐺 , (2.16)
onde 𝑅(𝑞𝐺) descreve a orientação do sensor na plataforma, 𝑅𝑇(𝑞) é utilizada para transformar
do sistema de coordenadas globais da plataforma para o sistema de coordenadas locais do
sensor. Ainda temos 𝑏𝐺 ∈ ℝ3, que é 𝑜𝑓𝑓𝑠𝑒𝑡 do sensor e representa o valor constante da
diferença entre o valor medido e o valor real e 𝛿𝐺 representa o ruído do sensor, que possuí média
zero e covariância 𝑄𝐺.
11
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 𝑎𝐴 ∈ ℝ3, definido em 𝑂𝑆, 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:
𝑎𝐴 = 𝑅(𝑞𝐴)𝑅𝑇(𝑞)((𝑎 + 𝑔) + 𝐻(𝜔, 𝛼)𝑜𝐴) + 𝑏𝐴 + 𝛿𝐴, (2.17)
onde 𝑅(𝑞𝐴) e 𝑅𝑇(𝑞) 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 𝑜𝐴 ∈ ℝ3 representa o offset do
sensor em relação origem de 𝑂𝐼. Os efeitos das forças inercias são dadas por (VANCIN, 2016):
𝐻(𝜔, 𝛼) = 𝑆(𝜔)2 + 𝑆(𝛼). (2.18)
12
Por fim, 𝑏𝐴 ∈ ℝ3 é 𝑜𝑓𝑓𝑠𝑒𝑡 de medição do sensor e 𝛿𝐴 representa o ruído integrado na medição,
que possuí média zero e covariância 𝑄𝐴.
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 𝑞 e posição 𝑝, onde um ponto 𝜉𝑖 ∈ ℝ3,
referenciado nas coordenadas globais, tem sua projeção no plano da imagem dado por 𝑚𝑖 ∈
ℝ2. Baseado em (MARIOTTINI e PRATTICHIZZO, 2005), obtém-se a seguinte relação:
𝑍𝑐�̃�𝑖 = 𝐾𝐶𝑅(𝑞𝐶)𝑅𝑇(𝑞)(𝜉𝑖 − 𝑝), (2.19)
onde �̃�𝑖 = [𝑚𝑖 1]𝑇 é a posição em pixels da projeção do ponto 𝜉𝑖 no frame de imagem, 𝑅(𝑞𝐶)
e 𝑅𝑇(𝑞) são matrizes de rotação utilizadas para orientar a câmera em relação ao sistema de
coordenadas globais e 𝐾𝐶 ∈ ℝ3×3 é a matriz com os parâmetros intrínsecos, dada por:
13
𝐾𝐶 = [𝑓𝑠𝑥 𝑓𝑠𝜃 𝑜𝑥0 𝑓𝑠𝑦 𝑜𝑦0 0 1
]. (2.20)
Os termos da matriz 𝐾𝐶 são parâmetros de calibração da câmera, onde (𝑜𝑥, 𝑜𝑦) é a coordenada
em pixels do ponto principal no plano da imagem (intersecção entre o plano da imagem e o eixo
𝑧𝑐), 𝑠𝑥 e 𝑠𝑦 representam o número de pixels por unidade de distância nas coordenadas da
imagem, 𝑓 é a distância focal e 𝑓𝑠𝜃 representa a inclinação do pixel (MA, SOATTO, et al.,
2012).
Por fim, 𝜉𝑖 pode ser descrito pelas coordenadas da câmera, de acordo com a Figura 6,
onde 𝜉𝑖 = [𝑋𝑐, 𝑌𝑐, 𝑍𝑐]𝑇. O parâmetro 𝑍𝑐 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 𝑆(�̃�𝑖)�̃�𝑖 = 0, resultando em:
0 = 𝑆(�̃�𝑖)𝐾𝐶𝑅(𝑞𝐶)𝑅𝑇(𝑞)(𝜉𝑖 − 𝑝). (2.21)
14
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.
15
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 𝑚𝑖 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.
16
Diagrama 2 - Processo de validação dos modelos utilizados.
Fonte: O autor.
A cinemática direta é um processo que busca estimar posição �̅� e orientação �̅� 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 �̅� e �̅� 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.
�̅�, �̅�
�̅�, �̅�
�̅�, �̅�
17
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.
Fonte: O autor.
�̅�, �̅�
�̂�, �̂�
𝑒𝑟𝑟𝑜
18
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
4.1.1 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 𝑀𝐻𝑧,
com 128 𝐾𝑏𝑦𝑡𝑒𝑠 de memória flash e 16 𝐾𝑏𝑦𝑡𝑒𝑠 de memória SRAM, configurando ao módulo
alta capacidade de processamento. Por fim, o microcontrolador também possui suporte em
19
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.
20
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.
4.1.2 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, desenvolveu-
se um protocolo de comunicação próprio para a plataforma de Stewart, que pode ser utilizado
21
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.
4.1.3 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,
22
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 𝑔, ou seja, mede acelerações de até 16 vezes a
aceleração da gravidade, enquanto o giroscópio possui fundo de escala de ±2000 𝑑𝑝𝑠. Outro
fator determinante para a escolha do sensor é sua taxa de amostragem e ambos os sensores
trabalham com altas taxas de até 3,33 𝑘𝐻𝑧. 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 𝑀𝐻𝑧, com 14 𝐾𝑏𝑦𝑡𝑒𝑠 de memória flash e 4 𝐾𝑏𝑦𝑡𝑒𝑠 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 11.
23
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.
4.1.4 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
24
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
25
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 𝑓(𝑥, 𝑦), pode ser descrito em função de
seus momentos invariantes (TEAGUE, 1980), onde:
𝑀00 = ∑ ∑ 𝑓(𝑥, 𝑦)𝑦𝑥 , (3.01)
26
é o momento de ordem zero e representa a área da imagem. Os momentos de primeira ordem
são dados por:
𝑀10 = ∑ ∑ 𝑥𝑓(𝑥, 𝑦)𝑦𝑥 , (3.02)
𝑀01 = ∑ ∑ 𝑦𝑓(𝑥, 𝑦)𝑦𝑥 . (3.03)
Com isso, podemos encontrar o centroide da imagem (�̅�, �̅�), onde �̅� = 𝑀10/𝑀00 e �̅� =
𝑀01/𝑀00, que no contexto desse trabalho é coordenada do marcador. A Figura 13(d) mostra o
resultado do processo descrito, onde a coordenada 𝑚𝑖 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)
(c)
(d)
Fonte: O autor.
𝒎𝑖
27
4.2 Validação dos Modelos
4.2.1 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 𝐽 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 𝑘 do valor medido 𝐿𝑚𝑒𝑑𝑖𝑑𝑜𝑖 de tamanho dos atuadores com 𝑖 = (1,… ,6).
Fluxograma 2 - Processo de minimização da função de custo através da função fminsearch do MATLAB.
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
𝑝𝑥, 𝑝𝑦, 𝑝𝑧 , 𝛼, 𝛽, 𝛾
𝐵𝑃, 𝑆𝑃, 𝐿𝑒𝑠𝑡𝑖𝑚𝑎𝑑𝑜
𝐿𝑚𝑒𝑑𝑖𝑑𝑜 − 𝐿𝑒𝑠𝑡𝑖𝑚𝑎𝑑𝑜 argmin 𝐽 < Ε
𝑝𝑜𝑠𝑖çã𝑜 = [𝑝𝑥, 𝑝𝑦, 𝑝𝑧]
𝑜𝑟𝑖𝑒𝑛𝑡𝑎çã𝑜 = [𝛼, 𝛽, 𝛾]
28
de um valor inicial. Neste contexto, valores iniciais de posição (𝑝𝑥, 𝑝𝑦 e 𝑝𝑧) 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 𝐵𝑖 e 𝑇𝑖 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:
𝐵𝑃𝑖 = 𝐵𝑖𝑅(𝑞𝐵) + [𝑝𝑥 𝑝𝑦 0]
𝑇,
𝑇𝑃𝑖 = 𝑇𝑖𝑅(𝑞𝑇) + [𝑝𝑥 𝑝𝑦 𝑝𝑧]𝑇,
(4.01)
onde 𝑞𝐵 e 𝑞𝑇 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 𝒑𝒙 = 𝟎. 𝟕𝟎, 𝒑𝒚 = 𝟎. 𝟓𝟎, 𝒑𝒛 = 𝟎. 𝟓𝟖, 𝜶 = 𝟎°, 𝜷 = 𝟎°
e 𝜸 = 𝟎°.
Fonte: O autor.
Dessa forma, podemos encontrar o valor estimado de tamanho dos atuadores, onde:
𝐿𝑒𝑠𝑡𝑖𝑚𝑎𝑑𝑜𝑖 = √(𝐵𝑃𝑖 − 𝑇𝑃𝑖)𝑇(𝐵𝑃𝑖 − 𝑇𝑃𝑖).
(4.02)
Com isso, temos:
29
𝐽 =∑(𝐿𝑚𝑒𝑑𝑖𝑑𝑜𝑖 − 𝐿𝑒𝑠𝑡𝑖𝑚𝑎𝑑𝑜𝑖)2.
6
𝑖=1
(4.03)
O algoritmo da função fminsearch estima a orientação e posição a partir de interações,
onde os valores de 𝑝𝑥, 𝑝𝑦, 𝑝𝑧, 𝛼, 𝛽 e 𝛾 são modificados até a condição dada pelo Fluxograma
2 ser estabelecida, onde Ε é o mínimo valor admitido para 𝐽. 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.
30
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
𝐹𝑎𝑐𝑡 e 𝐹𝑖𝑚𝑢, 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 𝐹𝑐𝑎𝑚;
5. Armazenamento dos dados de todos os sensores;
6. Obtenção da posição estimada �̅� e orientação estimada �̅� 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 �̅� e aceleração linear estimada �̅� 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 [𝐻𝑧]
Atuadores 𝐹𝑎𝑐𝑡 104
IMU 𝐹𝑖𝑚𝑢 104
Câmera 𝐹𝑐𝑎𝑚 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.
31
Figura 16 - Simulação do sensor giroscópio e comparação com os dados medidos.
Fonte: O autor.
Figura 17 - Simulação do sensor acelerômetro e comparação com os dados medidos.
Fonte: O autor.
32
Figura 18 - Simulação da câmera e comparação com os dados medidos.
Fonte: O autor.
4.3 Filtro de Kalman Estendido
A aplicação do Filtro de Kalman Estendido foi realizada em conjunto com os
pesquisadores do GACS utilizando o software MATLAB e o algoritmo apresentado na Tabela
1. As equações utilizadas no filtro encontram-se no Apêndice B. As estimativas apresentadas
aqui utilizam os dados dos sensores aplicados para a validação dos modelos, onde o tempo de
amostragem considerado é 𝑇𝑠 = (1/104) 𝑠. As comparações são realizadas utilizando as
estimativas feitas através da cinemática direta, possibilitando o cálculo dos erros de estimação.
Os valores iniciais dos estados 𝑞, 𝜔, 𝛼, 𝑝, 𝑣, 𝑎, 𝑏𝐺 e 𝑏𝐴 necessitam ser inicializados na
aplicação do filtro. Para os resultados da estimativa da Figura 19, os estados foram inicializados
com os valores conhecidos, pois o sistema parte de uma posição inicial de referência. Como
pode ser observado, o filtro faz a estimativa de posição (�̂�𝑥, �̂�𝑦 e �̂�𝑧) e orientação (�̂�, �̂� e 𝛾),
onde a dinâmica é estimada com grande precisão. Os erros obtidos podem ser visualizados nos
gráficos inferiores da mesma figura.
Por fim, a Figura 20 mostra os resultados obtidos quando o sistema parte de uma posição
inicial totalmente desconhecida, objetivando demostrar uma aplicação prática. Como pode ser
visto, apesar das condições iniciais dos estados serem desconhecidas, filtro converge
33
rapidamente e passa a estimar corretamente a dinâmica do sistema, onde o erro converge a zero
à medida que o tempo transcorre.
Figura 19 - Estimativa de posição e orientação através do Filtro de Kalman Estendido com posição inicial
conhecida.
Fonte: O autor.
Figura 20 - Estimativa de posição e orientação através do Filtro de Kalman Estendido com posição inicial
desconhecida.
Fonte: O autor.
34
5 CONCLUSÃO
Projetos na área de pesquisa e desenvolvimento são essenciais para a constante inovação
tecnológica da qual nossa sociedade demanda. Novas tecnologias são cada vez mais exploradas,
atingindo domínios distintos para os quais inicialmente haviam sido desenvolvidas. Dentro
desse contexto, o desenvolvimento desse trabalho foi de grande relevância para o aluno,
proporcionando um aperfeiçoamento de suas capacidades, tanto técnicas, através de
conhecimentos fundamentais da Engenharia Elétrica, como teóricas, com o estudo de temas que
não são abordados durante a graduação.
Neste trabalho foram desenvolvidos hardwares e firmwares necessários para a
instrumentação e sensoriamento de uma plataforma de Stewart, com foco na aplicação de um
algoritmo capaz de estimar a posição e orientação do topo da plataforma. Para a estimativa,
considerou-se o uso da fusão de uma IMU com uma câmera através de um Filtro de Kalman
Estendido baseado na representação por quatérnios. Os principais conceitos relativos ao filtro
foram abordados, possibilitando um entendimento do processo e também a base teórica
necessária para validar os modelos dentro das finalidades consideradas.
Na aplicação da metodologia proposta, foram demostrados os principais pontos levados
em consideração para o projeto dos hardwares desenvolvidos. As ferramentas necessárias para
efetuar o processamento de imagens foram detalhadas, possibilitando que este documento possa
ser utilizado como ponto de partida para trabalhos que possam vir a utilizar a fusão de uma
câmera para a correção de outros sensores. Um método para o cálculo da cinemática direta para
um sistema do tipo plataforma de Stewart foi apresentado possibilitando a validação dos
modelos através da simulação dos sensores e da comparação com os dados medidos, garantindo
assim uma descrição matemática do sistema utilizado. Por fim, os resultados da aplicação do
filtro foram apresentados objetivando validar o sistema como um todo.
Este trabalho considerou apenas a validação do algoritmo capaz de estimar a posição e
orientação do topo da plataforma de Stewart, abrindo diversas possibilidades para trabalhos
futuros. A estimativa dada pelo filtro viabiliza, por exemplo, um controle com feedback da
plataforma. Para isso, a implementação do algoritmo do Filtro de Kalman Estendido em tempo
real é de vital importância e pode ser explorada, tanto a nível de graduação, como pós-
graduação. Por ser um algoritmo genérico de estimativa de posição e orientação para sistemas
6-DOF, torna-se uma ferramenta que pode de ser utilizada em diversas áreas.
35
6 APÊNCIDES
6.1 Apêndice A
Aqui são apresentadas as principais funções utilizadas no processamento de imagens
para a obtenção da posição dos marcadores. As funções fazem parte do conjunto de bibliotecas
do OpenCV e são detalhas em função dos parâmetros utilizados durante o desenvolvimento
desse trabalho e são referenciadas em (BRADSKI e KAEHLER, 2008).
1. cv::cvtColor (imgRGB, imgHSV, COLOR_BGR2HSV);
Está função é usada para converter de um padrão de representação para outro. O
parâmetro de entrada da função é o vetor imgRGB da imagem capturada, que é convertido do
padrão HSV para RGB e armazenado em imgHSV. O padrão de conversão é dado por
COLOR_BGR2HSV, que faz parte dos códigos de conversão dessa função.
2. cv::inRange (imgHSV, Scalar(lowBoundsHue, lowBoundsSat, lowBoundsVal),
Scalar(upBoundsHue, upBoundsSat, upBoundsVal), imgTHR);
Está função avalia os elementos de um vetor de entrada imgHSV e compara para definir
se fazem parte de um intervalo determinado pelos parâmetros que definem esses limites
(segundo e terceiro parâmetros). O parâmetro de saída imgTHR é um vetor binário, onde os
elementos que fazem parte dos limites recebem valor ‘1’; caso contrário recebem valor ‘0’.
3. cv:: findContours (imgTHR, contours, mode, method);
Está função retorna os contornos de uma imagem binária imgTHR e armazena em um
vetor de ponteiros contours baseado em um modo mode e um método method. Com mode =
CV_RETR_CCOMP, os contornos são organizados em dois níveis de hierarquia (um
contendo os limites externos e o outro os limites internos). Por fim, method define o algoritmo
de aproximação.
4. moments = cv::moments(contour);
Está função calcula todos os momentos até terceira ordem de um polígono, neste caso
um dos contornos contour do vetor de contornos contours. Os momentos das Equações (3.01),
(3.02) e (3.03) podem ser obtidos pelos parâmetros de saída moments.m00, moments.m10,
moments.m01, respectivamente.
36
6.2 Apêndice B
Aqui são apresentados os modelos utilizados no algoritmo do Filtro de Kalman
Estendido baseados na teoria abordada previamente. Da mesma forma que o filtro pode ser
divido em duas etapas, os modelos também são apresentados dessa maneira, de acordo com as
etapas de predição e correção.
A. Predição
Para representar o sistema é necessário descrever sua dinâmica em função dos termos
apresentados na Equação (2.15). Considere o vetor de estados 𝑥 ∈ ℝ25 que representa essa
dinâmica e os vetores com os valores de offset dos sensores inerciais:
𝑥 =
[ 𝑞𝜔𝛼𝑝𝑣𝑎𝑏𝐴𝑏𝐺]
. (6.01)
Com isso o sistema pode ser representado em função de suas derivadas:
�̇� = 𝑔(𝑥) =
[ 𝐹(𝜔)𝑞𝛼03𝑣𝑎03×103×103×1 ]
=
[ 𝐺(𝑞)𝜔𝛼03𝑣𝑎03×103×103×1 ]
, (6.02)
onde a derivada do quatérnio é representada em função de 𝐹(𝜔) ∈ ℝ4×4 e 𝐺(𝑞) ∈ ℝ3×4, dados
por:
𝐹(𝜔) =1
2[0 −𝜔𝑇
𝜔 −𝑆(𝜔)] e 𝐺(𝑞) =
1
2[
−휀𝑇
𝜂𝐼3 + 𝑆(휀)]. (6.03)
O modelo contínuo pode ser discretizado baseado no método de Euler Forward, onde
𝑇 é período de amostragem:
�̇� =𝑥𝑘+1 − 𝑥𝑘
𝑇𝑠. (6.04)
Da Equação (2.4), podemos obter a Matriz Jacobiana 𝐺𝑘 ∈ ℝ25𝑥25:
𝐺𝑘(𝑥𝑘) = 𝐼25 + 𝑇𝑠 𝑑𝑖𝑎𝑔{Φ(𝑥𝑘),Ψ, 09×6}, (6.05)
onde 𝐼25 é uma matriz identidade de ordem 25 e:
37
Φ(𝑥𝑘) = [𝐹(𝜔𝑘) 𝐺(𝑞𝑘) 04×303×4 03×3 𝐼303×4 03×3 03×3
], Ψ = [
03×3 𝐼3 03×303×3 03×3 𝐼303×3 03×3 03×3
]. (6.06)
Por fim a matriz de covariâncias 𝑅𝑘 pode ser representada por:
𝑅𝑘 = 𝑇𝑠2 𝑑𝑖𝑎𝑔{𝑄𝑅,𝑞 , 𝑄𝑅,𝜔, 𝑄𝑅,𝛼, 𝑄𝑅,𝑝, 𝑄𝑅,𝑣, 𝑄𝑅,𝑎, 𝑄𝑅,𝑏𝐺 , 𝑄𝑅,𝑏𝐴}, (6.07)
onde 𝑄𝑅,(.) representa a covariância considerada para cada estado do modelo.
B. Correção
O Filtro de Kalman Estendido efetua o passo de correção baseado em um giroscópio,
um acelerômetro e uma câmera. O vetor 𝑧 ∈ ℝ12 que representa os sensores pode ser descritor
por,
𝑧 = [
𝜔𝐺𝑎𝐴
02𝑁×1 ], (6.08)
onde 𝑁 é o número de marcadores existentes e o modelo não-linear de saída é:
ℎ(𝑥) =
[
𝑅(𝑞𝐺)𝑅𝑇(𝑞)𝜔 + 𝑏𝐺
𝑅(𝑞𝐴)𝑅𝑇(𝑞)((𝑎 + 𝑔) + 𝐻(𝜔, 𝛼)𝑜𝐴) + 𝑏𝐴
𝐶𝐶 𝑆(�̃�1)𝐾𝐶𝑅(𝑞𝐶)𝑅𝑇(𝑞)(𝜉1 − 𝑝)
⋮𝐶𝐶 𝑆(�̃�𝑁)𝐾𝐶𝑅(𝑞𝐶)𝑅
𝑇(𝑞)(𝜉𝑁 − 𝑝) ]
, (6.09)
onde 𝐶𝐶 é uma matriz auxiliar usada para converter o vetor tridimensional para um vetor
bidimensional, dada por:
𝐶 = [1 0 00 1 0
]. (6.10)
A matriz de covariâncias 𝑄𝑘 da medição dos sensores é:
𝑄𝑘 = 𝑑𝑖𝑎𝑔{𝑄𝑄,𝜔𝐺 , 𝑄𝑄,𝑎𝐴 , 𝑄𝑄,𝐶1 , … , 𝑄𝑄,𝐶𝑁}, (6.11)
onde 𝑄𝑄,(.) representa a covariância considerada para cada sensor.
A Matriz Jacobiana 𝐻𝑘 ∈ ℝ12×25 das derivadas parciais de ℎ(𝑥𝑘) é dada por:
𝐻𝑘(𝑥𝑘) = [Γ𝑘(𝑥𝑘) Ξ𝑘(𝑥𝑘) Π𝑘(𝑥𝑘) Ω𝑘(𝑥𝑘)], (6.12)
onde:
Γ𝑘(𝑥𝑘) =
[
𝑅(𝑞𝐺)𝐽(𝑞𝑘, 𝜔)
𝑅(𝑞𝐴)𝐽(𝑞𝑘, 𝑎𝑘 − 𝑔)
𝐶𝐶 𝑆(�̃�1𝑘)𝐾𝐶𝑅(𝑞𝐶)𝐽(𝑞𝑘, (𝜉1 − 𝑝𝑘))
⋮𝐶𝐶 𝑆(�̃�𝑁𝑘
)𝐾𝐶𝑅(𝑞𝐶)𝐽(𝑞𝑘, (𝜉𝑁 − 𝑝𝑘))]
, (6.13)
38
Ξ𝑘(𝑥𝑘) =
[
𝑅(𝑞𝐺)𝑅𝑇(𝑞𝑘) 03×3
𝑅(𝑞𝐺)𝑅(𝑞𝑘)𝑇𝑊(𝜔𝑘, 𝑜𝐴) −𝑅(𝑞𝐴)𝑅(𝑞𝑘)
𝑇𝑆(𝑜𝐴)02×3 02×3⋮ ⋮
02×3 02×3 ]
, (6.14)
Π𝑘(𝑥𝑘) =
[
03×303×3
−𝐶𝐶 𝑆(�̃�1𝑘)𝐾𝐶𝑅(𝑞𝐶)𝑅(𝑞𝑘)𝑇
⋮−𝐶𝐶 𝑆(�̃�𝑁𝑘
)𝐾𝐶𝑅(𝑞𝐶)𝑅(𝑞𝑘)𝑇]
, (6.15)
Ω𝑘(𝑥𝑘) =
[ 03×3 03×3 𝐼3 03×303×3 𝑅(𝑞𝐴)𝑅
𝑇(𝑞𝑘) 03×3 𝐼302×3 02×3 02×3 02×3⋮ ⋮ ⋮ ⋮
02×3 02×3 02×3 02×3]
. (6.16)
As derivadas parciais são dadas por:
𝐽(𝑞, 𝑢) ≔
𝜕𝑅(𝑞)𝑢
𝜕𝑞=𝜕(𝐼3 − 2𝜂𝑆(휀) + 2𝑆
2(휀))𝑢
𝜕 [𝜂휀]
= 2[−𝑆(휀)𝑢 𝑆(𝑢)𝜂 +𝑊(휀, 𝑢)] ∀ 𝑢 ∈ ℝ3,
(6.17)
onde:
𝜕𝑆(𝑦)𝑢
𝜕𝑦= −𝑆(𝑢) ∀ 𝑦, 𝑢 ∈ ℝ3 (6.18)
e
𝑊(𝑦, 𝑢) ≔𝜕𝑆2(𝑦)𝑢
𝜕𝑦= 𝑦𝑢𝑇 − 2𝑢𝑦𝑇 + 𝑦𝑇𝑢𝐼3 ∀ 𝑦, 𝑢 ∈ ℝ
3. (6.19)
Por fim, a estimativa do quatérnio de orientação 𝑞 deve ser normalizada depois de ambos
os passos de predição e correção do filtro (MARKLEY e CRASSIDIS, 2014), onde:
�̅� =𝑞
||𝑞|| (6.20)
6.3 Apêndice C
Aqui são apresentados os esquemáticos resultantes dos projetos do módulo central e do
módulo da IMU, respectivamente.
39
40
41
42
7 REFERÊNCIAS
BAEK, J. et al. Visualizing Quaternion Multiplication. IEEE Access, 2017.
BRADSKI, G.; KAEHLER, A. Learning OpenCV: Computer vision with the OpenCV
library. [S.l.]: "O'Reilly Media, Inc.", 2008.
CARDONA, M. A new approach for the forward kinematics of general stewart-gough
platforms. Central American and Panama Convention (CONCAPAN XXXV), 2015 IEEE
Thirty Fifth, p. 1-6, 2015.
CHOSET, H. Principles of robot motion: theory, algorithms, and implementation.
[S.l.]: MIT press, 2005.
CORIOLIS FORCE. HyperPhysics, 2017. Disponivel em: <http://hyperphysics.phy-
astr.gsu.edu/hbase/corf.html>. Acesso em: 5 Novembro 2017.
DE FARIA, P. F. Quaternion-based dynamic control of a 6-DOF Stewart Platform for
periodic disturbance rejection. Pontifícia Universidade Católica do Rio Grande do Sul,
2016.
DIEBEL, J. Representing attitude: Euler angles, unit quaternions, and rotation vectors,
v. 58, p. 1-35, 2006.
HAMILTON, W. R. Ii. on quaternions; or on a new system of imaginaries in algebra.
Philosophical Magazine Series 3, v. 25, p. 10-13, 1844.
KUIPERS, J. B. Quaternions and rotation sequences. [S.l.]: Princeton university
press, v. 66, 1999.
KUMRA, S.; SAXENA, R.; MEHTA, S. Design and development of 6-DOF robotic
arm controlled by Man Machine Interface. Computational Intelligence & Computing
Research (ICCIC), 2012 IEEE International Conference on, p. 1-5, 2012.
MA, Y. et al. An invitation to 3-d vision: from images to geometric models. Springer
Science \& Business Media, v. 26, 2012.
MARIOTTINI, G. L.; PRATTICHIZZO, D. EGT for multiple view geometry and visual
servoing: robotics vision with pinhole and panoramic cameras. IEEE Robotics \& Automation
Magazine, v. 12, p. 26-39, 2005.
MARKLEY, F. L.; CRASSIDIS, J. L. Fundamentals of spacecraft attitude
determination and control. [S.l.]: Springer, v. 33, 2014.
MELLO, C. B. Controle de Trajetória de uma Plataforma Stewart para Simulação de
Transferência de Carga Fora de Porto. Universidade Federal do Rio de Janeiro, Rio de
Janeiro, 2011.
43
NÜTZI, G. et al. Fusion of IMU and vision for absolute scale estimation in monocular
SLAM. Journal of intelligent \& robotic systems, v. 61, p. 287-299, 2011.
STEVEN C, F. et al. The Mechanical Universe: Mechanics and Heat. [S.l.]:
Cambridge University Press, 2008.
STEWART, D. A platform with six degrees of freedom. Proceedings of the institution
of mechanical engineers, v. 180, p. 371-386, 1965.
TEAGUE, M. R. Image analysis via the general theory of moments. Journal of the
Optical Society of America (1917-1983), v. 70, p. 920-930, 1980.
THRUN, S.; BURGARD, W.; FOX, D. Probabilistic robotics. [S.l.]: MIT press, 2005.
UNDERWATER Navigation System, 2017. Disponivel em:
<http://www.ece.montana.edu/seniordesign/archive/SP14/UnderwaterNavigation/Quaternions
.html>. Acesso em: 14 out. 2017.
VANCIN, P. Fusão de Imagens e Sensores Inerciais para a Estimação e Controle de
Veículos Autônomos. Pontifícia Universidade Católica do Rio Grande do Sul, Porto Alegre,
2016.
XU, D.; LI, Y. F. A new pose estimation method based on inertial and visual sensors
for autonomous robots. Robotics and Biomimetics, 2007. ROBIO 2007. IEEE International
Conference on, p. 405-410, 2007.
ZHANG, K. et al. Fusion of vision and IMU to track the racket trajectory in real time.
Mechatronics and Automation (ICMA), 2017 IEEE International Conference on, p. 1769-
1774, 2017.
