XII Simpósio Brasileiro de Automação Inteligente (SBAI) Natal RN, 25 a 28 de outubro de 2015

Transcrição

1 Natal RN, 25 a 28 de outubro de 2015 CONTROLE DE POSIÇÃO DE UM VANT UTILIZANDO DIFERENTES FREQUÊNCIAS DE ATUALIZAÇÃO DAS OBSERVAÇÕES NA FUSÃO SENSORIAL Milton César Paes Santos, Lucas Vago Santana, Alexandre Santos Brandão, Mário Sarcinelli-Filho Instituto Federal do Espírito Santo - IFES, Campus Santa Teresa Santa Tereza, ES, Brasil Instituto Federal do Espírito Santo - IFES, Campus Linhares Linhares, ES, Brasil Universidade Federal de Viçosa - UFV Viçosa, MG, Brasil Universidade Federal do Espírito Santo - UFES Vitória, ES, Brasil s: milton.santos@ifes.edu.br, lucas@ifes.edu.br, alexandre@ufv.br, mario.sarcinelli@ufes.br Abstract This paper proposes the use of a decentralized filter to fuse sensorial data provided by a 3D capture system, based on a depth camera, with the inertial data provided by the embedded inertial measurement unit of an unmanned aerial vehicle (UAV), in order to position it at a desired point (such data are the observations inputted to the filter). In addition, the paper discusses the use of different frequencies for acquiring the data provided by the capture system, for control purposes. First of all, the UAV used in the work is presented, together with a simplified mathematical model to represent its dynamics. Following, a non-linear position controller is designed to keep the UAV hovering in a desired point. After that, the capture system developed for getting the UAV position through processing the depth images is presented. In the sequel a method to merge the data provided by the depth sensor and the data provided by the inertial sensor is implemented. The results obtained with the acquisition of the depth sensor data with low frequencies are compared with data obtained by inertial sensors, in experiments involving disturbance and abrupt maneuvers, to induce estimation errors in the system. Finally, a discussion of the importance of acquiring information at high frequencies is presented. Keywords Unmanned Aerial Vehicle, Quadrotor, Decentralized Kalman Filter Resumo Este trabalho propõe a utilização de um filtro descentralizado para realizar a fusão dos dados sensoriais provenientes de um sistema de captura 3D, baseado em uma câmera de profundidade, com os dados inerciais do sistema embarcado de um veículo aéreo não tripulado (VANT), a fim de posicioná-lo em um ponto desejado (tais dados são as observações fornecidas ao filtro). Adicionalmente, o trabalho aborda a utilização de diferentes frequências de aquisição de dados do sistema de captura para fins de controle. Primeiro, é apresentado o VANT utilizado no trabalho e um modelo matemático simplificado para representar sua dinâmica. Posteriormente, um controlador não linear de posição é desenvolvido para manter o VANT em voo pairado sobre um ponto desejado. Após isso, é apresentado o sistema de captura desenvolvido para obter o posicionamento do VANT através do processamento de imagens de profundidade. A seguir, um método para fusionar os dados provenientes do sensor de profundidade com dados fornecidos pelos sensores inerciais é implementado. Os resultados obtidos com aquisição dos dados do sensor de profundidade com baixas frequências são comparados com os dados obtidos pelos sensores inerciais, em experimentos envolvendo manobras bruscas e pertubações, para induzir erros de estimativa no sistema. Por fim, uma discussão sobre a importância da aquisição de informações em frequências altas é apresentada. Palavras-chave Veículos aéreos não tripulados, Quadrimotores, Filtro de Kalman descentralizado 1 Introdução Nos dias atuais, para ter acesso a áreas de difícil acesso ou de risco é mais conveniente a utilização de robôs, para preservação da integridade humana. Os veículos aéreos não tripulados (VANTs) de pás rotativas são excelentes robôs para operar nessas regiões (Szafranski et al., 2013). Alguns modelos possuem múltiplos motores, como é o caso dos quadrimotores, por exemplo, que é um dos drones mais utilizados hoje em dia. Eles são capazes de decolar e pousar verticalmente, de se manter pairados em um ponto qualquer do espaço, e de realizar manobras, versatilidade esta que facilita seu uso em qualquer tipo de ambiente, interno ou externo. O controle de posição do VANT é uma tarefa fundamental para automatizar sua navegação, e já foi implementado em diversos trabalhos (Engel et al., 2012; Santana et al., 2013). Entretanto, durante a realização dos experimentos observa-se que a estimação da posição do VANT com base nos sensores internos do veículo engloba erros de odometria tridimensional, o que impede que o veículo execute a tarefa de posicionamento com êxito. Como solução, propõe-se a utilização de um sistema de captura externo para obter o posicionamento 3D do VANT e, com isso, aprimorar sua localização (Santos et al., 2015). 515

2 Esta ideia de adicionar mais sensores ao sistema para incorporar suas informações na estimação de estado é conhecida como fusão sensorial (Mutambara, 1998). O GPS é utilizado para localização do veículo durante experimentos em ambientes externos (Christophersen et al., 2006) e câmeras para experimentos em ambientes internos (Engel et al., 2012; Santana et al., 2013). Em ambos os casos, as frequências de aquisição dos dados são inferiores aos 100 Hz, podendo chegar até mesmo a 1 Hz, como no caso de alguns GPS. Neste contexto, este trabalho apresenta alguns resultados obtidos para diferentes frequências de aquisição, com foco na atualização das observações para estimação da posição do VANT, para finalmente realizar o seu controle. Para tratar de tais temas, a Seção 2 apresenta o modelo e o controlador PD utilizados para controle de posição do VANT. Prosseguindo, a Seção 3 apresenta o sistema de captura, incluindo a detecção do VANT e as conversões necessárias para calcular sua posição 3D. A seguir, a Seção 4 apresenta o Filtro de Kalman Descentralizado (FKD) utilizado para realizar a fusão sensorial e assim aprimorar a estimação da posição e das velocidades do VANT. Por fim, os resultados obtidos durante o controle de posição, com diferentes frequências de aquisição do sistema de captura dos dados, são apresentados na Seção 5. 2 Modelo do VANT Neste trabalho é utilizado um VANT de pás rotativas do tipo quadrimotor, o AR.Drone 2.0 Power Edition R, da Parrot, Inc. Tal VANT é mostrado na Figura 1, juntamente com os sistemas de coordenadas adotados. Uma vantagem de se trabalhar com este veículo está na sua resposta quando recebe sinais de comando, que se dá através de uma relação linear (Santana et al., 2014). Os sinais de comando u representam a entrada real (aqueles efetivamente aplicadas ao VANT), e são uż, que corresponde a um comando de velocidade linear, e causa deslocamento no eixo z; u ψ, que corresponde a um comando de velocidade angular, e causa rotação ao redor do eixo z; u φ, que corresponde, indiretamente, a um comando de velocidade linear, e causa deslocamento no eixo y (assim, u φ = uẏ); u θ, que corresponde, indiretamente, a um comando de velocidade linear, e causa deslocamento no eixo x (assim, u θ = uẋ). As variáveis de estado do VANT são definidas como q = [ξ η], onde ξ = [x y z] T R 3 representa os deslocamentos longitudinal, lateral e normal, e η = [φ θ ψ] T R 3 contém os ângulos de rolagem, arfagem e guinada. Neste trabalho, um modelo ainda mais simplificado do que o proposto em (Santana et al., 2014) será aplicado para representar o AR.Drone, considerando que o objetivo de controle é permanecer em voo pairado em uma posição desejada ξ d = [x d, y d, z d ] T. Para tanto, os sinais de controle são U = [u θ, u φ, uż] T, e o modelo do VANT é ξ = κ u U κ v ξ, (1) onde ξ = [ẍ, ÿ, z] representa as acelerações do VANT nos eixos x, y e z, respectivamente, do sistema de coordenadas do veículo, enquanto κ u, κ v R 3x3 são matrizes cujos elementos são constantes de proporcionalidade identificadas experimentalmente. O modelo apresentado em (1) está escrito com base no referencial do veículo, e deve ser convertido para o referencial global, tornando-se, assim, onde κ u = ξ = R w κ u U R w κ v ξ, (2) K K K 3, κ v = K K K 6 (3) e R w é a matriz de rotação definida utilizando-se a orientação do VANT (ψ) em relação ao sistema de coordenadas global, dada por, cosψ sinψ 0 R w = sinψ cosψ 0. (4) Controle de Posição 3D Figura 1: O quadrimotor AR.Drone 2.0 PowerEdition R, da Parrot, Inc., e os sistemas de coordenadas adotados ({w} é o sistema global e {b} é o do veículo). Nesta subseção é apresentado o controlador não linear proposto para guiar o VANT para uma posição desejada (Santana et al., 2014). Logo, o objetivo de controle é conduzir a aeronave da sua posição atual ξ = [ x y z ] T para a posição desejada ξ d = [ x d y d z d ] T, aplicando-lhe comandos de velocidade. Reescrevendo (2), obtém-se ξ = f 1 U f 2 ξ, (5) 516

3 onde ξ = ẍ ÿ, U = u v x u vy, e ξ = ẋ ẏ, z uż ż com f 1 e f 2 sendo as duas matrizes 3 3 de (2), dadas, respectivamente, por R w κ u e R w κ v. Utilizando a técnica de dinâmica inversa é proposto um controlador, similar àquele proposto em (Santana et al., 2014). Para isso, adota-se a lei de controle U = f 1 1 (ν + f 2 ξ), (6) com ν = ξ d + κ d ξ + κp ξ, onde ξ = ξd ξ é o erro de posição, K px 0 0 K dx 0 0 κ p = 0 K py 0 e κ d = 0 K dy K pz 0 0 K dz (7) são as matrizes de ganhos proporcionais e derivativos, respectivamente. A Figura 2 mostra as variáveis e um esquemático da tarefa desejada. zr zd1 z <o> y x yr yd1 x r,y r,zr xr 26,22 xd1 d x d,y d,z z ~ d x ~ y ~ ~ ~ ~ ~ x,y,z Figura 2: VANT buscando o ponto desejado ξ d. Substituindo a lei de controle (6) em (5), obtém-se a equação que governa a dinâmica do erro de posição como ξ + κd ξ + κp ξ = 0. (8) Na sequência, para analisar a estabilidade do sistema, a função candidata de Lyapunov V( ξ, ξ) 1 = 2 ξ T 1 κ p ξ + ξ T ξ 0 (9) 2 é definida. Calculando sua primeira derivada e substituindo (8), tem-se V( ξ, ξ) = ξt κ p ξ + ξt ξ = ξ T κ p ξ ( ) + ξt κ p ξ κd ξ = ξt κ d ξ 0. (10) Portanto, a função V( ξ, ξ) é semi-definida negativa. Logo, pode-se concluir pela Teoria de Lyapunov para sistemas não lineares que ξ 0 com t. Adicionalmente, aplicando o teorema de La Salle à (8), pode-se finalmente garantir que ξ 0 com t. Portanto, o sistema em malha fechada é assintoticamente estável. 3 Sistema de Captura Neste trabalho foi montado um ambiente com um sistema de captura, usando um sensor de profundidade Xtion Pro Live para capturar a posição do VANT. As imagens de profundidade são adquiridas a partir de um feixe de luz infravermelha estruturado, sendo, portanto, invariante à intensidade de iluminação do ambiente. Ao processar essas imagens com mapa de profundidade estruturado, determina-se uma relação da altura em que o VANT se encontra com a área que os blobs ocupam na imagem, fazendo-se, assim, a detecção do quadrimotor, conforme detalhado em (Santos et al., 2015). Quando se utiliza um sensor de profundidade (RGB-D), é necessário realizar conversões de 2D (mapa da imagem) para 3D (referência global). Os métodos de conversão e uma explicação com mais detalhes também são apresentados em (Santos et al., 2015). Tal conversão é dada por ξ c = Ξ <c,w> x img (11) Ξ <c,w> = ḡ 1 P A 1 z ij, onde A, P, ḡ e z ij são a matriz dos parâmetros intrínsecos da câmera, a matriz de projeção, a matriz de parâmetros extrínsecos da câmera e o pixel da linha i e coluna j da matriz de profundidade, respectivamente. Figura 3 mostra as coordenadas de transformação e as posições de referência. j i 2D zij T <c,w> [i,j,1] z <o> y x yc Xtion xc Zc yb Yc Xc zb 3D zij c xc yc zij Figura 3: Transformações e referências. Em (a) é apresentado o plano 2D, conforme visão da câmera. Os sistemas de referência são < b >, o referencial do VANT, < c >, o referencial a câmera (Xtion Pro Live), e < o >, o referencial global. 4 Fusão Sensorial Para estimar a posição 3D do VANT é necessário obter as informações de posição em uma referência global. Neste trabalho, serão utilizadas as informações provenientes da odometria (calculadas por integração numérica dos dados dos sensores inerciais (IMU)) e as posições obtidas através dos dados visuais do sistema de captura. Para executar a fusão dessas informações, pode-se utilizar o Filtro de Kalman - KF (do inglês Kalman Filter) (Kalman, 1960) ou o Filtro de Kalman Descentralizado - FKD (Mutambara, 1998), este último utilizado para a fusão de múltiplos sensores/sistemas. xb 517

4 4.1 Filtro de Kalman O Filtro de Kalman requer que o modelo de sistema seja x k+1 = A k x k + w k z k = H k x k + υ k, (12) onde x k representa o vetor de estado no instante k, A k é a matriz que relaciona a transição dos estados x k para x k+1, z k é o vetor de observações, H k é a matriz que descreve a conexão entre z k e x k, e, finalmente, w k e v k são vetores de ruído do sistema e das observações, respectivamente. Considerando uma estimação inicial do processo como ˆx k k 1 no instante t k, as equações correspondentes ao Filtro de Kalman são Predição: Estimação: ˆx k k 1 = A k ˆx k 1 P k k 1 = A k P k A T k + Q k (13) k = (P k k 1 ) 1 + H T R 1 H k ˆx k = ˆx k k 1 + K k ( zk H k ˆx k k 1 ) K k = P k H T k R 1 k, (14) onde P k k 1 é a previsão de P k (dadas as observações até o instante k 1), P k é a matriz de covariância do erro entre o estado real x k e o estado estimado ˆx k e K k é a matriz de ganho do Filtro de Kalman. 4.2 Filtro de Kalman Descentralizado Um sistema descentralizado consiste em uma rede de filtros, com unidades de processamento individuais para cada filtro. Nestes sistemas, a fusão ocorre localmente em cada unidade, baseada na transmissão de informações das unidades vizinhas que são enviadas para os filtros locais. Em um sistema de fusão sensorial descentralizado, o processamento do filtro é um nó do sistema, que distribui as observações e as informações locais aos demais filtros. Assim, o filtro descentralizado usa essas informações para gerar novas saídas fusionadas. O algoritmo executado nos filtros ou nas unidades de processamento é aquele descrito na sequência. No instante k, para o i-ésimo filtro local, temse ( ˆx ik = P ik P k 1ˆx 1 k 1 + H ik Ri 1 ) z ik (15) i e para o filtro global tem-se ˆx k = P k [ n i=1 ik k = = k 1 + HT ik R 1 ik H ik, (16) ˆx ik (n 1) k 1 ˆx k 1 n i=1 ik ] (17) 1 (n 1)Pk 1. (18) 4.3 Estimação da Posição 3D O vetor de estimação dos estados do FKD é ˆx = [ x y z ẋ ẏ ż ]. (19) O diagrama da Figura 4 representa as matrizes dos filtros utilizados na fusão das informações provenientes da câmera de profundidade e dos sensores inerciais(imu) do VANT. Figura 4: Diagrama do sistema de estimação das posições e velocidades do VANT com FKD. É importante ressaltar que na implementação do FKD pode-se criar um mecanismo que controla quando as observações de cada filtro local serão incorporadas no filtro global. Na Figura 4 a frequência de atualização das observações da câmera é limitada pela chave 1. Assim, incorporam-se as informações da câmera ao filtro global em determinados instantes, conforme a taxa de aquisição. 5 Resultados Experimentais Os resultados a seguir apresentam o controle de posição executado conforme descrito na Seção 2. Utilizam-se as informações de saída do FKD para realizar a tarefa de posicionar o VANT na posição desejada. Também foram inseridos distúrbios manuais nas direções de controle, em todos os experimentos, a fim de validar o método proposto. As diferentes frequências de aquisição dos dados da câmera são evidenciadas nos intervalos e na quantidade de informações presentes nas Figuras 5(a), (b), (c) e (d), que mostram os resultados dos experimentos 1, 2, 3 e 4, respectivamente (as marcações com um círculo azul é que representam os dados obtidos pelo sistema de captura). No Experimento 1, a frequência de aquisição dos dados da câmera foi programada para atualizar o FKD a cada 1Hz. Entretanto, o filtro consegue estimar os estados de posição e velocidades da aeronave na taxa de 100Hz, devido às informações inerciais do VANT. Os erros nos instantes de [ , , ] foram ocasionados por pertubações aplicadas ao VANT. Os Experimentos 2, 3 e 4 foram realizados seguindo o mesmo procedimento, mas variando a frequência de aquisição dos dados da câmera para 1.5Hz, 6Hz e 30Hz, respectivamente, e os instantes em que as pertubações são inseridas. Para melhor interpretação dos dados e do 518

5 (a) (b) Note-se que o objetivo de controle foi alcançado em todos os experimentos, uma vez que o VANT permaneceu ancorado na posição desejada ξ d = [0, 0, 0.8], inclusive sob pertubações, conforme desejado. Assim, foi comprovado que independentemente da frequência de atualização das observações do filtro para correção do posicionamento tridimensional, desde que o sistema tenha algum outro sensor que forneça as informações de velocidade e posição à uma frequência de pelo menos 30Hz, que é a frequência mínima (f min ) para o envio de sinais de controle ao quadrimotor utilizado, o controle de posição é efetivamente realizado. Neste sentido, as informações do sistema de captura são incorporadas ao filtro e realizam uma correção nos dados de posição fornecidos pelos sensores inerciais, passíveis de erros expressivos ao longo do tempo (Santana et al., 2014). A manipulação de imagens é um processo custoso. Assim, no decorrer dos experimentos foi avaliado o tempo médio de processamento (T p ) de todo o algoritmo (detecção do VANT, atualização das observações, estimação dos estados e cálculo dos sinais de controle), rodando em um computador à base do processador I7 da Intel, de quatro núcleos, com memória RAM de 4 GBytes e sistema operacional Windows 7. A Tabela 1 apresenta os resultados obtidos. Tal tempo representa o tempo de execução total medido ao longo de cada experimento, dividido pelo número total de amostras de posição obtidas. Assim, quando a taxa de aquisição das imagens de profundidade é a mesma da odometria se obtém um tempo médio maior, pois o cálculo da fusão dos dados sensoriais é feito a cada loop de controle, o que não ocorre quando a taxa de aquisição das imagens de profundidade diminui. (c) (d) Figura 5: Resultados dos Experimentos 1, 2, 3 e 4 com as frequências de atualização das observações em 1Hz, 1.5Hz, 6Hz e 30Hz, respectivamente em (a), (b), (c) e (d). que realmente acontece com a aeronave na inserção de pertubações, foi editado um vídeo com todos os experimentos deste artigo. Logo, para obter mais detalhes, o leitor deve acessar o link Tabela 1: Tempos de execução médio de cada iteração do sistema, para várias frequências de captura das imagens de profundidade. Exp. freq [Hz] T p [ms] O Experimento 5, ilustrado na Figura 6, tem o mesmo objetivo de controle dos demais experimentos. A diferença é a realização de uma manobra brusca (entre os instantes [ ]) e a amplitude das pertubações introduzidas(nos intervalos entre [ ] e [ ]). Destaquese que após a realização da manobra brusca as informações dos sensores inerciais acumulam um erro que compromete a execução da tarefa desejada. Porém, mesmo com erros no cálculo da posição e velocidade, o VANT se mantém ancorado na posição desejada, visto que as informações visuais da câmera auxiliam na correção destes cálculos a cada iteração. A Figura 6(a) mostra a evolução da posição do VANT ao longo das iterações. Observe-se que ξ r ξ d, assim como ξ 0, como vê na Figura 6(b), conforme previsto. 6 Conclusões Apesar do sistema de odometria do VANT tender a fornecer valores cada vez mais errados, ao longo do tempo, esses dados respeitam o comportamento do movimento, porém deslocados. Essa observação é importante, já que o controle do 519

6 ao IFES (Instituto Federal do Espírito Santo), à UFV (Universidade Federal de Viçosa) e à FA- PEMIG, por viabilizarem sua participação neste trabalho. Referências (a) (b) Figura 6: Resultado do Experimento 5 com a frequência de atualização fixada a 30Hz. Em (a) e (b), são mostradas as posições e velocidades obtidas no experimento, respectivamente. VANT se baseia nos erros de sua posição em relação ao ponto desejado ξ d. Logo, se não houvesse uma correção da posição, a tarefa de controle seria executada com êxito, mas para efeitos de experimentos práticos seria observado um contínuo deslocamento lateral, ocasionado pelo erro acumulativo de posição da odometria (drifting). Em virtude disso, os experimentos reais com diferentes frequências de atualização da posição foram executados com sucesso, mesmo com frequências bastante baixas de atualização de posição. Em teoria, pode-se afirmar que se os sensores inerciais do VANT fornecessem os dados de velocidade sem imprecisões a frequência de aquisição da câmera (f c ) no filtro de fusão poderia ser muito baixa, comparada à frequência de atuação do VANT (f V ANT ), ou seja f c << f V ANT, que mesmo assim a tarefa de controle seria executada. Entretanto, na prática observou-se que ao excitar o VANT a realizar manobras bruscas (flip ahead) os dados inerciais de velocidade acumulam um erro interno que ocasiona a má estimação das velocidades e posições. Neste caso, se f c f V ANT a estimação da posição do VANT fica comprometida, desestabilizando o sistema, levando-o ao chão. Logo, para experimentos práticos, recomenda-se incorporar ao filtro de fusão a maior quantidade de informações de posição tridimensional do VANT possível. Agradecimentos Os autores agradecem ao CNPq - Conselho Nacional de Desenvolvimento Científico e Tecnológico, e à FAPES - Fundação de Amparo à Pesquisa e Inovação do Espírito Santo, pelo apoio financeiro ao projeto. Eles também agradecem à UFES (Universidade Federal do Espírito Santo), Christophersen, H. B., Pickell, W. R., Neidoefer, J. C., Koller, A. A., Kannan, S. K. and Johnson, E. N. (2006). A compact guidance, navigation, and control system for unmanned aerial vehicles, Journal of Aerospace Computing, Information, and Communication pp Engel, J., Sturm, J. and Cremers, D. (2012). Camera-based navigation of a low-cost quadrocopter, Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Kalman, R. E. (1960). A new approach to linear filtering and prediction problems, ASME Journal of Basic Engineering. Mutambara, A. G. (1998). Decentralized Estimation and Control for Multisensor Systems, CRC Press. Santana, L. V., Brandao, A. S., Sarcinelli-Filho, M. and Carelli, R. (2014). A trajectory tracking and 3d positioning controller for the ar.drone quadrotor, 2014 International Conferenc eon Unmanned Aircraft Systems (ICUAS2014), Orlando, FL, USA, pp Santana, L. V., Sarcinelli-Filho, M. and Carelli, R. (2013). Estimation and control of the 3d position of a quadrotor in indoor environments, Proceedings of the 16th International Conference on Advanced Robotics (ICAR 13), Montevideo, Uruguay. Santos, M. C. P., Santana, L. V., Martins, M. M., Brandao, A. S. and Sarcinelli-Filho, M. (2015). Estimating and controlling uav position using rgb-d/imu data fusion with decentralized information/kalman filter, 2015 IEEE International Conference on Industrial Technology (ICIT 2015), Seville, Spain, pp Szafranski, G., Czyba, R., Janusz, W. and Blotnicki, W. (2013). Altitude estimation for the uav s applications based on sensors fusion algorithm, International Conference on Unmanned Aircraft Systems (ICUAS), pp