O documento descreve o desenvolvimento de um sistema de navegação indoor para um quadricóptero não tripulado usando imagens. O sistema usa medidas vetoriais obtidas por câmera e velocidade angular medida por girômetro para estimar a atitude do veículo usando um filtro de Kalman estendido para quatérnios. O sistema é avaliado por simulações computacionais.

1. Anais do XIX Encontro de Iniciação Científica e Pós-Graduação do ITA - XIX ENCITA / 2013
Instituto Tecnológico de Aeronáutica, São José dos Campos, SP, Brasil, 17 de outubro de 2013
DESENVOLVIMENTO DE UM SISTEMA NAVEGAÇÃO INDOOR POR IMAGENS PARA UM VEÍCULO AÉREO NÃO TRIPULADO DO TIPO QUADRICÓPTERO
Roberto Brusnicki
Instituto Tecnológico de Aeronáutica – ITA
Rua H8A – APT 119 – Campus do CTA – 12228-460
São José dos Campos/SP – Brasil
Bolsista PIBIC-CNPq
Correio eletrônico: rbrusnicki@gmail.com
Davi Antônio dos Santos
Instituto Tecnológico de Aeronáutica (ITA)
Pça. Marechal do Ar Eduardo Gomes, nº 50 - Vila das Acácias.
Bolsista PIBIC-CNPq
Correio eletrônico: davists@ita.br
Resumo. O emprego de câmera em sistemas navegação baixo custo para veículos aéreos não tripulados (VANTs) do tipo quadricóptero tem recentemente sido foco de muitas investigações. Porém, em trabalhos anteriores, medidas provenientes de câmera são acrescidas à estimação da posição/velocidade ou então usadas diretamente como realimentação para guiamento, mas não especificamente auxiliar no processo de determinação atitude. Este trabalho está envolvido na determinação de atitude VANTs do tipo multirotores usando medidas vetoriais obtidas por câmera. Supõe-se que o veículo está equipado com um girômetro de três eixos, e uma câmera voltada para solo fixada à base do mesmo. Também se assume que o voo será em ambiente fechado, contendo várias landmarks postas em posições conhecidas do solo. A quantidade e posição das landmarks são escolhidas de tal modo que ao menos duas sempre permaneçam no campo de visão da câmera. Assim, a cada instante tempo, dois vetores unitários não colineares direcionados a partir da câmera aos centroides de duas landmarks podem ser computados. A fim realizar a determinação de atitude, o método estimação adotado foi QEKF (quatérnion extended Kalman Filter). O sistema de determinação de atitude é avaliado por simulações computacionais.
Palavras chave: determinação de atitude, multirotores, filtro Kalman, visão computacional.
1. Introdução
A determinação de atitude (AD) é uma parte fundamental qualquer sistema controle para veículos aéreos não tripulados (VANTs). Em geral, interessa-se na estimação da atitude do veículo e de sua velocidade angular com relação à um dado sistema de coordenadas. As estimativas computadas pela função AD é então usada para prover as leis do controle de atitude com realimentação informação.
A literatura acerca de AD é muito extensa e tem sido desenvolvida principalmente no campo aeroespacial (Wertz, 1978), (Yang, 2012). Os métodos de AD derivam do Wabba Problem (Markley, 1988), que define uma rotina para estimar atitude a partir de medidas vetoriais. (Cheng et al., 2008) usa o filtro de Kalman estendido (EKF) para estimar ângulos de arfagem e rolagem um Micro Veículo Aéreo (MAV). A terceira coluna da Matriz de Cossenos Diretores (DCM) e o viés do girômetro são usados como variáveis de estado. A gravidade é usada vetor observado no modelo de medidas. O ângulo guinada é obtido do vetor campo geomagnético. Gebre-Egziabner e Elkaim (2008) usam ambos os campos gravitacional e geomagnético como vetores observados em duas diferentes abordagens de estimação de quatérnion. A primeira abordagem é um estimador iteração mínimos quadrados (LSE) e o segundo um EKF. O LSE executa uma pesquisa global da atitude, em cada passo de tempo. Pelo contrário, o algoritmo EKF computa por uma informação a priori, resultando em melhor performance. Os dois métodos acima foram designados serem livres de girômetros e assistidos por GPS. (Bar-Itzhack e Oshman, 1985) propõe um fitro de Kalman estendido para estimação de quatérnion (QEKF), que, garantir com norma unitária, realiza um passo normalização euclidiana após cada atualização de medidas. (Idan, 1996) propõe um filtro de mínima variância para estimar atitude parametrizada pelos parâmetros de Rodrigues (MRP) Schaub, 1998). Devido a expressões algébricas mais simples, esta abordagem possui uma vantagem computacional em relação os estimadores de quatérnion. (Markley and Crassidis, 1996) apresenta um filtro de Kalman estendido multiplicativo (MEKF) que estima erro atitude com os Parâmetros Modificados de Rodrigues (MRP) e atualiza a atitude total representada por quatérnion meio multiplicação de quatérnions.
Este trabalho apresenta um método de determinação atitude para VANT do tipo multirotor usando medidas vetoriais obtidas por imagem. Assume-se que o veículo está equipado com dois sensores presos à sua estrutura: uma

2. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
câmera voltada para o solo, e um girômetro de três eixos. Assume-se também que o veículo voará em ambiente fechado, sobre um solo com várias landmarks. Ambos, veículo e landmarks possuem posições conhecidas com relação ao sistema de coordenadas adotado. As landamarks são dispostas, em quantidade e posições, de tal modo que pelo menos duas delas sempre estejam no campo de visão da câmera (FOV). Usando medidas obtidas câmera, dois vetores não colineares medidos a partir da câmera ao centroide das landmarks podem ser computados. Com o intuito de obter um sistema para a determinação de atitude VANTs multirotores, estas medidas vetoriais, assim como velocidade angular são consideradas pelo método de estimação atitude QEKF (Bar-Itzhack and Oshman, 1985). O sistema proposto é verificado por simulações computacionais.
O restante do texto aqui apresentado está organizado da seguinte maneira: Seção 2, definação do problema; Seção 3, reformulação do método de estimação atitude; Seção 4, apresentação de alguns resultados de simulações; Seção 5, conclusões do trabalho; Seção 6, agradecimentos; Seção 7, referências bibliográficas.
2. Definição do problema
Considere o multirotor e os três sistemas de coordenadas cartesianas (CCS) ilustrado na Figura 1. O CCS do corpo SB = {XB, YB, ZB} está preso ao veículo em seu centro de massa (CM). O CCS do solo SL = {XL, YL, ZL} é fixado ao solo no ponto O. O CCS de referência SR = {XR, YR, ZR} é paralelo à SG, mas é centralizado no CM.
Figura 1: Os sistemas de coordenadas cartesianas à esquerda, e o ambiente voo direita.
Supõe-se que a câmera está posicionada no CM e os eixos do girômetro estão alinhados com SB. Define-se o conjunto de landmarks como sendo I ≜{1, 2, …, l}. Denota-se o centroide da i-ésima landmark por M(i). Define-se 푠⃗(푖) como sendo o vetor unitário com origem no CM e direcionado à M(i). Denota-se a representação de 푠⃗(푖) em SB e em SR por 풃(푖) 휖 ℝ³ e 풓(푖) 휖 ℝ³, respectivamente. A representação de 풃(푖) e 풓(푖) estão relacionadas por 풃(푖)=푫.풓(푖), onde 푫 흐 푆푂(3) é a matriz de atitude de SB em relação à SR. A fim de medir-se dois pares de vetores não colineares (풃(푖),풓(푖)), assume-se que ambos CM e landmarks tem posições conhecidas, e além disso, pelo menos duas landmarks são observadas pela câmera a cada instante de medição. Isto leva aos seguintes dois pares de medidas vetoriais:
휈푘≜{(풃̂ 풌 (푖1),풓̂풌 (푖1)),(풃̂ 풌 (푖2),풓̂풌 (푖2))}, 푖1 휖 I,푖2 휖 I,푖1 ≠ 푖2 (1)
Onde k denota o instante de tempo discreto, e para 푖=푖1,푖2:
풃̂ 푘 (푖)=푫(풂푘)풓푘 (푖)+훿풃푘 (푖) (2)
풓푘 (푖)=풓̂푘 (푖)+훿풓푘 (푖) (3)
Onde 풓̂푘 (푖) é uma amostragem de 풓(푖) no instante k, 훿풃푘 (푖) e 훿풓푘 (푖) são sequências de ruídos brancos Gaussianos de média nula com covariâncias 푹푏,푘 (푖) e 푹푟,푘 (푖) , respectivamente, e 풂푘 휖 ℝ푛 é a representação do vetor de atitude de tempo discreto que parametriza a matriz de atitude 푫(풂푘).
A cinética da atitude é modelada pela seguinte equação diferencial (Wertz, 1978):
풂̇(푡)=풇(풂(푡),흎(푡)) (4)

3. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
Onde 풂(푡) é a versão de tempo contínuo de 풂푘, 흎(푡) 휖 ℝ³ é a velocidade angular verdadeira. Já que o girômetro não é perfeito, a velocidade angular verdadeira é dada pelo seguinte modelo estocástico:
흎(푡)=흎̂(푡)+훿흎(푡) (5)
Onde 흎̂(푡) 휖 ℝ³ é a velocidade angular medida, e 훿흎(푡) é o ruído das medidas do girômetro, que é suposto ser uma ruído branco Gaussiano com média nula de covariância Q. Uma versão de tempo discreto da equação (5) é dada por 흎푘=흎̂푘+훿흎푘, onde 훿흎푘 tem a mesma característica 훿흎(푡), sendo no entanto uma sequência.
O problema principal deste trabalho consiste em determinar computacionalmente de maneira recursiva a estimativa 풂̂푘|푘 com mínima variância do verdadeiro vetor de atitude 풂푘 usando a Eq. (4), a sequência de medidas de velocidade angular 휔̂1:푘, e a sequência de medidas vetoriais 휈1:푘.
3. Solução do problema
Esta seção apresenta o método de estimação adotado para solucionar problema exposto acima: O Quaternion Extended Kalman Filter (Bar-Itzhack and Oshman, 1985), que consiste num filtro de Kalman estendido para estimação de quatérnio a partir medidas vetoriais. Para manter norma do estimado próxima da unitária, emprega-se simplesmente uma etapa de normalização Euclidiana das estimativas. Esse filtro será referido como QEKF. Em seguida, as equações do QEKF são revisadas com base em (Santos, 2008).
3.1. Estimação de quatérnion
Bar-Itzhack e Oshman (1985) propuseram um filtro de Kalman estendido tempo discreto para estimação quatérnion de atitude. Este método é descrito a seguir. Seja o vetor de estado 풂(푡) do estimador de quatérnio de rotação, QEKF, dado por:
풒(푡)≜[ 푞1,푡 풆푡 ]
(6)
onde 푞1,푡 e 풆푡 = [푞2 푞3 푞4]′ são as representações escalar e vetorial, respectivamente, dos componentes real e imaginário de q (sendo que ●′ define a matriz transposta de ●). Assim, tem-se que a equação de medidas não-linear discreta no tempo do estimador de quatérnio de rotação é:
풃푖,푘+1=푫(풒푘+1).풓푖,푘+1+훿풃푖,푘+1
(7)
onde, 푫(풒푘+1)=[ 푞12+푞22−푞32−푞422(푞2푞3+푞1푞4)2(푞2푞4−푞1푞3) 2(푞2푞3−푞1푞4)푞12−푞22+푞32−푞422(푞3푞4+푞1푞2) 2(푞2푞4+푞1푞3)2(푞3푞4−푞1푞2)푞12−푞22−푞32+푞42] 푘+1;
풃푖,푘+1: medidas vetoriais feitas no sistema de eixos cartesianos do VANT;
풓푖,푘+1: medidas vetoriais em relação ao sistema de referência;
훿풃푖,푘+1: ruído de medição.
O modelo de cinemática atitude para o quatérnio rotação é dado pela seguinte equação diferencial linear
풒̇(푡)=휴(푡).풒(푡)
(8)
onde,

4. Anais do XIX Encontro de Iniciação Científica e Pós-Graduação do ITA - XIX ENCITA / 2013
Instituto Tecnológico de Aeronáutica, São José dos Campos, SP, Brasil, 17 de outubro de 2013
휴(푡)= 12[ 0 휔1(푡) 휔2(푡) 휔3(푡) −휔1(푡) 0−휔3(푡) 휔2(푡) −휔2(푡) 휔3(푡) 0−휔1(푡) −휔3(푡) −휔2(푡) 휔1(푡) 0]
(9)
Em que 휔1(푡), 휔2(푡),휔3(푡) correspondem respectivamente as velocidades angulares do VANT em relação aos eixos 푥,푦 e 푧.
Integrando a Eq. (8) de 푡푘 a 푡푘+1, obtém-se:
풒푘+1=휱(푡푘+1,푡푘).풒푘
(10)
Onde 휱(푡푘+1,푡푘) 휖 ℝ4x4 é a matriz de transição de estado. Considerando-se que a velocidade angular 휔(푡)=[휔1(푡)휔2(푡)휔3(푡)]′ seja constante durante o intervalo de tempo 푇=푡푘+1−푡푘 entre amostragens sucessivas das medidas vetoriais, essa matriz é dada por:
휱(푡푘+1,푡푘)=푒훺푘.푇
(11)
onde 휴푘 é computada pela Eq. (9), mas utilizando 흎푘 em vez de 흎(푡). Substituindo, então, 흎(푡) por 흎̂(푡)+ 훿흎(푡) na equação anterior:
휱(푡푘+1,푡푘)=푒(훺̂ 푘+훿훺푘).푇=푒훺̂ 푘.푇.푒훿훺푘.푇
(12)
onde as matrizes 휴̂ 푘e 훿휴푘 são dadas pela Eq. (9), porém substituindo 흎(푡) por 흎̂푘 e 훿흎푘, respectivamente. Representando o segundo fator do lado direito da Eq. (12) pela correspondente série de potências, obtém-se:
휱(푡푘+1,푡푘)=푒훺̂ 푘.푇.(푰4+훿휴푘.푇+⋯)
(13)
Considerando-se que 훿흎푘e 푇 sejam ambos pequenos, o truncamento da série dada pela Eq. (13) após termos de primeira ordem permite que a Eq. (10) seja aproximada por:
풒푘+1≈푒훺̂ 푘.푇.풒푘+푒훺̂ 푘.푇.훿휴푘.푇.풒푘
(14)
Pode-se verificar que:
훿휴.풒= 12.휩.훿흎
(15)
onde,
휩≜[ −푞2 푞1 푞4−푞3−푞3−푞4 푞1 푞2−푞4 푞3−푞2 푞1]
(16)
Sendo assim, utilizando a Eq. (15) na manipulação do segundo termo do lado direito da Eq. (14), finalmente se obtém a seguinte equação de estado discreta no tempo:

5. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
풒푘+1=푒훺̂ 푘.푇.풒푘+ 푇 2 푒훺̂ 푘.푇.휩푘.훿흎푘
(17)
Onde 휩푘 é dado pela Eq. (16), mas utilizando o quatérnio verdadeiro no instante 푡푘,풒푘, no lugar de 풒. Para fins de implementação dos estimadores de quatérnio, a covariância 푸푘 푞 do ruído de estado (segundo termo à direita na Eq. (17)) é aproximada por:
푸푘 푞≅휞푘.푸.휞푘′
(18)
onde,
휞푘= 푇 2 푒훺̂ 푘.푇.휩̂ 푘
(19)
e 휩̂ 푘 é computada pela Eq. (16), porém utilizando 풒̂푘|푘 no lugar de 풒.
A exponencial matricial que define a matriz de transição de estado na Eq. (11) é aqui computada por:
푒훺푘.푇=cos(‖흎푘‖. 푇 2).푰4+ 1‖흎푘‖ 푠푒푛(‖흎푘‖. 푇 2).휴푘
(20)
O quatérnio de rotação apresenta norma unitária e, dessa forma, seus componentes obedecem à restrição 풒′.풒=1. No entanto, a atualização linear do KF, por envolver a operação de soma, não garante que as estimativas de 풒 mantenham essa propriedade ao longo da estimação. A forma mais simples de lidar com tal restrição consiste no uso de normalização Euclidiana das estimativas após o estágio de atualização aditiva.
Em seguida é apresentado o estimador QEKF, o qual utiliza normalização Euclidiana para garantir que as estimativas tenham normas unitárias.
3.2. QEKF
Mediante uso do EKF (Extended Kalman Filter) e tendo-se em vista a equação de estado linear (11) e a equação de medidas não linear (1), obtêm-se os estágios de propagação e de atualização do QEKF. Para garantir que as normas de suas estimativas sejam unitárias, utiliza-se normalização Euclidiana.
A estimativa normalizada , apresenta um erro cuja covariância é aproximada no QEKF simplesmente por:
푷푘+1|푘+1∗=푷푘+1|푘+1
(21)
O QEKF requer a matriz Jacobiana das medidas,
푯푖,푘+1 푞= 휕푫(풒).풓푖,푘+1 휕풒 | 풒=풒̂푘+1|푘
(22)
A Tabela 1 apresenta o QEKF.

6. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
Tabela 1: Estimador de quatérnio rotação – QEKF (Santos, 2008 – pg. 31).
4. Simulações e resultados
O desempenho do estimador apresentado é avaliado inicialmente com medidas simuladas, e posteriormente com dados coletados em tempo real. Para o primeiro caso, a verdadeira atitude do multirotor qk é propagada pela Eq. (17) com as seguintes velocidades angulares:
휔푘=[ 0,1.푠푒푛(푘.푡) 0,1.cos (푘.푡) −0,1.푠푒푛(푘.푡).cos (푘.푡) ]
(23)
onde 푡=0,1푠. As medidas vetoriais da câmera foram geradas utilizando-se a Eq. (2), onde:
푟푘 (1)=[ 05/13−12/13]
(24)
푟푘 (2)=[ 0−5/13−12/13]
(25)

7. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
Ambas as covariâncias dos sensores, girômetro e câmera, foram configuradas de tal forma a não divergir estimação do filtro. Os valores tomados para a covariância ruído das medidas foram o girômetro, e 푹푏,푘 (푖)= (0,01)2.푰3 para a câmera. (Onde IN é a matriz identidade NN).
Usando os dados simulados, a rotina do QEKF foi executada para um período de tempo 100s, tendo portanto executado a rotina de estimação 1000 vezes. As condições iniciais utilizadas foram:
풒̂0|0=[1 0 0 0]′
(26)
푷0|0=0,01.푰4
(27)
onde se supôs que: 푞0~풩([1 0 0 0]′,푷0|0).
Acurácia e ortogonalidade foram os parâmetros examinados nesta fase. A acurácia é medida como se segue em (Wertz, 1978):
퐼푘=|arccos( 12[푡푟(푫(풒̂푘|푘) ′ .푫(풒푘))−1])|
(28)
onde o parâmetro 퐼푘 corresponde ao erro angular entre a atitude verdadeira e estimada na notação de ângulo principal de Euler.
A ortogonalidade é dada por:
퐽푘=푡푟{[푫(풒̂푘|푘) ′ .푫(풒푘)−퐼3] ′ .[푫(풒̂푘|푘) ′ .푫(풒푘)−퐼3]}
(29)
onde o parâmetro 퐽푘 descreve o quão perto a matriz de atitude estimada está uma ortogonal, tanto quanto mais próximo de zero.
Figura 2: Índice de ortogonalidade e erro angular em função do tempo. A escala vertical primeiro gráfico está multiplicada pelo fator 10−31.
A seguir, executou-se novamente a rotina com dados coletados partir dos sensores utilizados no projeto em tempo real. Dado que nesta simulação não se possui acesso ao valor real da atitude, verificou-se o desempenho do QEKF plotando-se os valores das inovações de todas as componentes ambos os vetores estimados para duas landmarks observadas pela câmera. Verificou-se que como esperado, estes gráficos possuem média nula e são descorrelacionados. Para cada um dos seis gráficos, plotou-se também o sigma correspondente (raiz quadrada do

8. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
respectivo elemento da diagonal principal matriz de covariância inovação). Os dados foram coletados enquanto os sensores executavam movimentos similares aos quais estariam sujeitos no voo de multirotores. Foi-se executado a rotina durante 30s, sendo cada medida obtida a 0,03s.
Figura 3: Em azul, valores da inovação de cada uma das componentes ambos os vetores obtidos. E verde, respectivo valor de sigma, proveniente da matriz covariância inovação. Sendo de cima para baixo, as coordenadas de x, y e z, sendo os 3 gráficos iniciais correspondentes aos valores da primeira landmark e os últimos 3 da segunda.
5. Conclusões
O método de estimação atitude apresentado obteve ótimos resultados nas simulações. Pode-se verificar que este método é adequado para ambientes (fechados, entre outros) em que não se possui acesso à GPS, dado se utiliza do mesmo. Uma alternativa para voos em lugares abertos é utilizar juntamente com as medidas vetoriais da câmera, os vetores correspondentes à direção da gravidade e do campo geomagnético terra. Sendo o teste em voo sem dúvida uma fase crucial do projeto, está sendo preparado e se espera ser executado em breve a fim de alcançar-se o objetivo pré-estabelecido, isto é, desenvolver um sistema de navegação baixo custo, porém excelente qualidade. Ainda assim, a idealização do sistema, e verificação de sua viabilidade, desempenho já foram plenamente confirmadas neste projeto.

9. Anais do XIX ENCITA, ITA, 17 de outubro de 2013
,
6. Agradecimentos
Agradeço ao Prof. Davi Antônio dos Santos por ter cedido seu tempo e paciência fornecer auxílio no desenvolvimento deste projeto, e pelo seu sempre constante incentivo. Ao Instituto Tecnológico de Aeronáutica ensino de qualidade, sem o qual este trabalho não teria sido possível, e pelas inúmeras outras oportunidades oferecidas. Agradeço ainda em especial ao CNPq que financiou este projeto através do programa PIBIC. Este incentivo é de suma importância ao desenvolvimento da pesquisa científica no Brasil.
7. Referências
Bar-Itzhack, I.Y. and Oshman, Y., 1985. “Attitude determination from vector observations: Quaternion estimation”. IEEE Transactions on Aerospace and Electronic Systems, Vol. 21, pp. 128–136.
Cheng, L., Zhaoying, Z. and Xu, F., 2008. “Attitude determination for MAVs using a kalman filter”. Tsinghua Science
and Technology, Vol. 13, pp. 593–597.
Gebre-Egziabher, D. and Elkaim, G.H., 2008. “MAV attitude determination by vector matching”. IEEE Transactions on
Aerospace and Electronic Systems, Vol. 44, pp. 1012–1028.
Idan, M., 1996. “Estimation of rodrigues parameters from vector observations”. IEEE Transactions on Aerospace and
Electronic Systems, Vol. 32, pp. 578–586.
Markley, F.L., 1988. “Attitude determination using vector observations and the singular value decomposition”. Journal
of the Astronautical Sciences, Vol. 38(3), pp. 245–258.
Markley, F.L. and Crassidis, J.L., 1996. “Attitude estimation using modified rodrigues parameters”. In Proceedings of the Flight Mechanics/Estimation Theory Symposium. Greenbelt, USA.
Santos, D. A. Estimação de Atitude e Velocidade Angular Satélites Utilizando Medidas do Campo Geomagnético da Direção do Sol. Dissertação de Mestrado. Instituto Tecnológico de Aeronáutica, São José dos Campos, 2008.
Schaub, H. Novel Coordinates for Nonlinear Multibody Motion with Applications to Spacecraft Dynamics and Control, 1998. Tese de Doutorado – Texas A&M University.
Wertz, J.R., 1978. Spacecraft Attitude Determination and control. Kluwer Academic Publishers, The Netherlands.
Yang, Y., 2012. “Spacecraft attitude determination and control: Quaternion based method”. Annual Reviews in Control, Vol. 36, pp. 198–219.GELB, A. Applied Optimal Estimation. Boston: M.I.T. Press, 1974.