Estimación garantista de la posición de un quadrotor con GPS

ESTIMACI ´ ON GARANTISTA DE LA POSICI ´ ON DE UN QUADROTOR CON GPS Ram´on A. Garc´ıa, Manuel G. Ortega y Francisco R. Rubio Depto. de Ingenier´ıa de Sistemas y Autom´atica, Universidad de Sevilla,{ramongr, mortega, rubio}@us.es Guilherme V. Raffo Depto. de Engenharia Eletrˆonica, Unisersidade Federal de Minas Gerais, [email protected] Resumen La aplicaci´on de dos m´etodos de estimaci´on garantistas sobre un quadrotor se presentan en este art´ıculo. Se tiene un modelo del sistema discretizado en el que se calcula su posici´on cada Tsegundos, mientras que la medida del GPS es cada tsincro segundos. Se aplicaron los algoritmos garantistas para contemplar las posibles posiciones en las que se puede encontrar el quadrotor y posteriormente, gracias a la medida del sensor corregir y mejorar la estimaci´on realizada. Palabras clave: Estimaci´on garantista, aritm´etica intervalar, zonotopos, quadrotor, GPS. 1. Introducci´on Un quadrotor es un veh´ıculo a´ereo con cuatro motores coplanarios. En la Fig.1 se observa un esquema con las distintas fuerzas y momentos que act´uan sobre dicho tipo de veh´ıculo. Figura 1: Esquema de las Fuerzas y Pares sobre un Quadrotor. Se trata pues de un sistema continuo cuya medici´on de la posici´on a trav´es de un GPS se realiza de forma discreta. Se tienen dos tiempos de muestreo, uno debido a la implementaci´on del observador discreto, T, y otro con el que se realizan las mediciones del GPS, tsincro, donde T < tsincro. En la Fig.2 se puede apreciar el comportamiento que se plantea: en el instante de partida t1se tiene tanto la estimaci´on de la posici´on a trav´es de las ecuaciones del modelo como la medida de la se˜nal GPS, y hasta que no han transcurridos tsincro segundos no se volver´a a tener la sincronizaci´on con el GPS. Mientras que en los instantes de tiempo intermedios, cada Tsegundos, se tendr´a la estimaci´on basada en las ecuaciones del modelo del sistema. El uso de dos tiempos de muestreo ya ha sido empleado en [11], donde se trabaja con un tiempo para el modelo y otro para la medida del sensor, aunque con fines distintos. Figura 2: Ejemplo instantes de muestreo utilizados. Por otro lado, el modelo presenta distintas incertidumbres que pueden provenir de diversas fuentes, tales como error al medir la masa o la distribuci´on de la misma, din´amica del sistema no contemplada, imposibilidad de medir perturbaciones como viento, efecto suelo, etc. Tambi´en existe la posibilidad de tener incertidumbres a la hora de realizar la medida por parte de los sensores, como muestra la Fig.3. Si se considera una medida totalmente precisa nos encontraremos en el caso (a). Si por el contrario, se considera que la medida no sea perfecta y se asume que puede tener cierta inexactitud, se tiene el caso (b). M´as en detalle, en la Fig.3 se representa un proceso de estimaci´on, donde en un momento o instante determinado se realiza una medici´on con el GPS o sincronizaci´on. Considerando un sistema lineal con una fuerza externa tambi´en lineal, se tiene debido a dicho proceso para cada coordenada en el caso (a) un tri´angulo, ya que al sincronizar nos fiamos completamente de la medida y por tanto al “resetear” tenemos un solo punto, mientras que para el caso (b) lo que se obtiene es un trapecio, ya que al “resetear” no se tiene un punto sino un conjunto de puntos posibles en los que se puede encontrar el estado. Figura 3: Ejemplos de medida GPS. A la hora de estimar las variables de estado de un sistema, existen diferentes m´etodos posibles. Entre ellos, los observadores de estado, usados en diferentes aplicaciones, por ejemplo [1], [12], [8], [14] y [5]. Bas´andonos en la clasificaci´on realizada en [6] podemos resaltar tres tipos: 1. El observador de Luenberger que no trata expl´ıcitamente las incertidumbres. 2. Estimadores basados en el comportamiento estad´ıstico, como el filtro de Kalman que presupone el ruido con cierta funci´on de probabilidad. 3. Estimadores basados en estados garantistas, los cuales no necesitan presuponer una funci´on de probabilidad sino que se basan en que las variables estar´an acotadas dentro de un determinado rango, es decir, son conjuntos compactos (en Rnque sean acotados y cerrados). Como ejemplo de este ´ultimo tipo, si se usaran intervalos como m´etodo de estimaci´on garantista, sin la necesidad de suponer una funci´on de probabilidad como se hace en el caso del filtro de Kalman, s´olo hay que tener en cuenta que el ruido o la incertidumbre est´an acotados entre un valor m´aximo y uno m´ınimo. De este modo, no es necesario recurrir a un estudio estad´ıstico de dicho ruido, lo que en ocasiones puede ser dif´ıcil o tedioso de calcular. Continuando con [6], los estimadores garantistas dan como resultado un estado estimado que es un dominio en el espacio de estados. Dicho domino representa una cota exterior de todos los posibles estados que son consistentes tanto con el modelo incierto (con incertidumbres y/o ruido) como con las medidas inciertas (con incertidumbres y/o ruido). Tal dominio se puede representar de diversas maneras, ya sea mediante elipsoides, cajas (provenientes del uso de aritm´etica intervalar), paralelotopos o incluso politopos de complejidad limitada, es decir, con un n´umero de v´ertices y caras limitado. Seg´un el m´etodo empleado para representar el dominio, se tendr´a de mayor o menor manera el llamado efecto wrapping1, es decir, ser´a m´as o menos conservador en funci´on de c´omo se acoten las soluciones. No es lo mismo acotarlo por una caja de lados paralelos a los ejes, que por un paralep´ıpedo o por un zonotopo. En la Fig.4 se puede observar el conjunto de estados posibles, la acotaci´on externa mediante una caja cuyos lados son paralelos a los ejes (supuestos ejes en horizontal y vertical), y un paralelep´ıpedo cuyos lados no son paralelos a los ejes y que se intenta adaptar mejor a la forma del conjunto de estados posibles. Se puede apreciar como la soluci´on obtenida por medio de la caja es m´as conservadora que la obtenida por el paralep´ıpedo. Figura 4: Ejemplo de ajuste del conjunto de posibles estados. En este art´ıculo se pretende dise˜nar un observador que estime el estado del sistema haciendo uso de las medidas obtenidas del mismo. El resto del trabajo est´a organizado de la siguiente manera, en la secci´on 2 se presenta el modelo del sistema. Los dos algoritmos usados ser´an introducidos en la secci´on 3 y posteriormente se ver´an los resultados obtenidos mediante simulaci´on a un caso de estudio en la secci´on 4. Finalmente, las conclusiones se muestran en la secci´on 5. 1El efecto wrapping es el hecho de a˜nadir m´as soluciones de las reales al utilizar un m´etodo de acotaci´on demasiado conservador y por tanto obtener un conjunto de posibles estados mayor que los posibles estados reales que podr´ıa alcanzar el sistema. 2. Modelado del Sistema Partiendo del caso general, sea un sistema discreto con incertidumbres, tanto en el modelo como en la medida, dado por: xk+1 =f(xk, wk) yk=g(xk, vk)(1) donde xk∈Rnes el vector de estado del sistema, yk∈Rpes el vector de salida, wk∈Rnwrepresenta errores de modelado as´ı como perturbaciones y finalmente vk∈Rpvrepresenta el vector de ruidos en las mediciones. Para los m´etodos que se van a emplear en la resoluci´on de la estimaci´on, se supone que las incertidumbres est´an acotadas dentro de unos valores concretos, sin importar la distribuci´on estad´ıstica. Por ello se asume que wk∈W,vk∈Vyx0∈X0. Particularizando (1) en el caso de que se desee estimar la posici´on espacial de un quadrotor, se parte de la segunda Ley de Newton, considerando que todas las fuerzas se agrupan en una sola componente. Siendo an´alogas las expresiones para los tres ejes, si se considera la expresi´on s´olo para la componente x, se tendr´a ¨x=Fx m. Traslad´andolo al espacio de estados, se pueden definir dos estados, x1=xyx2= ˙x Se tiene pues un sistema descrito por la posici´on y la velocidad en R6:         ˙x1 ˙x2 ˙x3 ˙x4 ˙x5 ˙x6         =               ˙x ˙y ˙z Fx m Fy m Fz m               (2) donde x, y, z son las coordenadas en posici´on, ˙x, ˙y, ˙zson las velocidades lineales, mes la masa del veh´ıculo y Fx, Fy, Fzson fuerzas aplicadas en cada uno de los ejes respectivamente. Si se expresa (2) en tiempo discreto, el resultado ser´ıa:         x1k+1 x2k+1 x3k+1 x4k+1 x5k+1 x6k+1         =         x1k+T x4k x2k+T x5k x3k+T x6k x4k x5k x6k         +                    T2 2 Fx m T2 2 Fy m T2 2 Fz m TFx m TFy m TFz m                    (3) donde Tes el tiempo de muestreo empleado. Se han mostrado de forma reducida las fuerzas aplicadas, si se desarrollan incluyendo el subsistema de rotaci´on de un quadrotor, se pueden expresar las fuerzas aplicadas como: Fx= (cos φcos ψsin θ+ sin φsin ψ)u+Ax Fy= (cos φsin ψsin θ−sin φcos ψ)u+Ay Fz= (cos φcos θ)u+Az (4) siendo φ, θ, ψ los ´angulos de Euler (roll, pitch y yaw respectivamente), uel empuje total y Aila perturbaci´on en el eje icorrespondiente. Sin embargo en (3) se ha empleado una notaci´on simplificada en lugar de hacer referencia a los ´angulos de Euler o al empuje, ya que eso es objeto de c´alculo del controlador de estabilizaci´on el cual no se est´a abordando en esta parte, es decir, aqu´ı se calculan Fx, Fy, Fznecesarias para el movimiento traslacional y a partir de dichas fuerzas se calcular´ıa en el control rotacional los ´angulos y empuje necesarios. Una vez presentado el sistema, se ver´a c´omo se ha abordado la resoluci´on del problema. Dado el sistema se quiere estimar su posici´on a trav´es de un modelo discreto con incertidumbres y una medici´on de la posici´on del mismo, que podr´a ser a su vez incierta o no, seg´un el caso de estudio. 3. Algoritmos utilizados A continuaci´on se mostrar´an los dos casos objetos de estudio: aritm´etica intervalar y zonotopos. Se ha considerado el movimiento en el plano XY, reordenando los estados de (3) y sin usar las componentes correspondientes al eje z. 3.1. Aritm´etica Intervalar La idea de utilizar la aritm´etica intervalar en estimaci´on garantista surge de la partida del conocimiento de los l´ımites de las incertidumbres, por ejemplo, se conoce que puede haber un error m´aximo y m´ınimo a la hora de calcular la masa, pero no se sabe exactamente cuanto es el valor del error. Visto el motivo de su aplicaci´on, se ver´a el sistema reordenado a partir de (3):     x1k+1 x2k+1 x3k+1 x4k+1     =    x1k+T x3k x2k+T x4k x3k x4k     +            T2 2 Fx m T2 2 Fy m TFx m TFy m            (5) donde x1es la posici´on x,x2es la posici´on y,x3 es la velocidad ˙xyx4es la velocidad ˙y. El resto de la notaci´on es la utilizada en (3). Para abordar el problema, se hace uso de una adaptaci´on del algoritmo presentado en [9], donde se hace uso de la aritm´etica intervalar para estimar el estado de un sistema aut´onomo (xk+1 =f(xk)). Esquem´aticamente, el algoritmo es el siguiente: Paso 1: [ˆx](t2) = [x](t1) + (t2−t1)f([x](t1)) Paso 2: [v] = [x](t1)∪[ˆx](t2) Paso 3: [w] = inflado([v], αω[v] + β) Paso 4: [x](t2) = [x](t1)+(t2−t1)f([w]) donde ω[v] es la longitud de su lado mayor y la operaci´on de inflado dado una caja [x] y un escalar es: inflado([x], ) = [x1−, x1+]×... ×[xn− , xn+] Como resultado negativo de usar un m´etodo tan conservador como el propuesto con la aritm´etica intervalar es el llamado efecto wrapping. En la Fig.4 se puede observar c´omo la funci´on f est´a acotada por la caja de color negro cuando en realidad hay zonas de dicha caja en las que no cae ning´un punto. La fuente de incertidumbre en el modelo se ha supuesto que sea un error a la hora de la medici´on de la masa, m. Debido a ello se tendr´a un intervalo en el que se podr´an encontrar los distintos estados. La funci´on de inclusi´on2fde (5) ser´a (por simplicidad en la notaci´on se han omitido los sub´ındices k): f([x]) =      [x1, x1] + T[x3, x3] + T2 2[1 m,1 m]Fx [x2, x2] + T[x4, x4] + T2 2[1 m,1 m]Fy [x3, x3] + T[1 m,1 m]Fx [x4, x4] + T[1 m,1 m]Fy      (6) 2Seg´un otros autores se puede expresar como [f] siendo Tel tiempo de muestreo. Se han elegido α= 0,1 y β= 0,0001. A la hora de calcular ω[v] se han separado los estados en funci´on a la coordenada que representen, es decir, x1yx3por un lado yx2yx4por otro. Una vez hecha la distinci´on, se aplica a cada pareja de forma separada aquel que sea el mayor de cada par. 3.2. Zonotopos La idea es utilizar otro procedimiento que permita una estimaci´on garantista cuyo resultado sea una acotaci´on menos conservadora. Se vuelve a partir del conocimiento de los l´ımites de la incertidumbre, pero esta vez no se suponen que tengan forma rectangular, o de caja, sino que se permite una forma m´as compleja, haciendo usos de politopos, concretamente de zonotopos. En esta ocasi´on, el sistema reordenado a partir de (3), es el siguiente:     x1k+1 x2k+1 x3k+1 x4k+1     =    x1k+T x2k x2k x3k+T x4k x4k     +            T2 2 Fx m TFx m T2 2 Fy m TFy m            (7) donde x1es la posici´on x,x2es la velocidad ˙x,x3 es la posici´on yyx4es la velocidad ˙y. El resto de notaci´on es la ya utilizada en (3). Seg´un el algoritmo mostrado en [4]: Paso 1: Usar una funci´on de inclusi´on para acotar la trayectoria del sistema no lineal con incertidumbres. Paso 2: Calcular una cota del conjunto de estados consistente. Paso 3: Calcular una cota ajustada del conjunto intersecci´on. Para el paso 1, se emplear´a el m´etodo de K¨uhn [10], con el cual se consigue un zonotopo que acota los posibles estados del sistema. Para el paso 2, a partir de las medidas obtenidas se estimar´a una franja del conjunto de variables de estados factibles para dicha medici´on. Y por ´ultimo para el paso 3 habr´a que calcular la intersecci´on entre zonotopo y franja. Por tanto, lo que se ver´a en este momento es c´omo calcular una franja de posibles estados a partir de una medici´on. En [4] viene detallado c´omo obtener un conjunto de estados a trav´es de la medida tomada. Para ello se tendr´an tantas franjas como componentes de la salida; siendo la salida yk=g(xk, vk) donde xk∈Rnes el estado del sistema y vk∈Rpves el ruido en la medida. Supongamos que tenemos una medida yk∈Rp, se tiene como conjunto de estados consistentes con la medida, denotado por Xyk: Xyk={x∈Rn:yk∈g(x, V )} Se define Xyk(i) como el conjunto de estados consistentes con la componente i-´esima de yk: Xyk(i) = {x∈Rn:yk(i)∈gi(x, V )} donde gi(x, V ) denota la i-´esima componente de g(x, V ). Con ambas definiciones tendremos que: Xyk⊆ p \ i=1 Xyk(i) En [4] se demuestra por qu´e una franja sirve de cota externa para el intervalo, en este art´ıculo nos quedaremos con el resultado, y el procedimiento para obtener a partir de una medida una franja. Se supone dado un zonotopo ˆ Xky una medida yk. Se deben obtener mediante aritm´etica intervalar el vector ci∈Rny los escalares si, σi∈Rtal que: ci=mid ∇xgi(ˆ Xk, V ) cT iˆ Xk−gi(ˆ Xk, V )⊆[si−σi, si+σi] entonces, si Xe yk(i)=x:cT ix−yk(i)−si≤σi resulta que la intersecci´on entre el zontopo ˆ Xky el conjunto de estados consistentes i-´esimo pertenecer´a a la intersecci´on entre el zonotopo ˆ Xky la franja ´ı-´esima Xe yk(i): ˆ XkTXyk(i)⊆ˆ XkTXe yk(i) 4. Simulaciones Una vez vistos los algoritmos, se mostrar´an los resultados obtenidos en simulaci´on. 4.1. Aritm´etica Intervalar Una de las ventajas de usar intervalos, es que la intersecci´on de dos intervalos sigue siendo un intervalo. Este hecho, sin embargo, no ocurre con los zonotopos. Realizando una simulaci´on cuya fuerza aplicada consiste en un escal´on de subida y otro de bajada para volver al punto inicial, sin incertidumbres en la medida, se obtiene como resultado el mostrado en la Fig.5. En la simulaci´on se cuenta con dos tiempos distintos, un tiempo de sincronizaci´on de 1 segundo (recordar que este tiempo es el que hay entre lectura y lectura del GPS) y uno de muestreo de 20 milisegundos (recordar que este tiempo es el que transcurre entre c´alculo y c´alculo del vector de estados). Figura 5: Coordenada X con GPS sin incertidumbres. Si ahora, la medida del GPS se considerada no “fiable” y se supone un dispositivo diferencial con un error de ±0,2mpor ejemplo, se puede observar en la Fig.6 como var´ıan los resultados. Al igual que en el caso “fiable” el tiempo de sincronizaci´on es de 1 segundo y el de muestreo de 20 milisegundos. Figura 6: Coordenada X con GPS con incertidumbres. 4.2. Zonotopos Al igual que con el caso de la aritm´etica intervalar se mostrar´an los resultados sobre el eje x, la fuerza aplicada consiste en un escal´on de subida y otro de bajada para volver al punto inicial y que as´ı se llegue a un estado en r´egimen permanente. Realizando la simulaci´on bajo dos condiciones, medida de la se˜nal GPS considerada exacta, y considerada con una desviaci´on de 0.2 metros se obtienen las Fig.7 y 8 respectivamente. En la Fig.7 se puede apreciar como al principio el ´area de la regi´on definida por el zonotopo crece hac´ıa un zonotopo mayor, pero conforme se reciben las se˜nales fiables del GPS se consigue reducir el tama˜no del zonotopo que relaciona la posici´on, coordenada x1, con la velocidad, coordenada x2. En la Fig. 8 se puede apreciar como en esta ocasi´on en la que las medidas del GPS no se consideran exactas, no se tiene una disminuci´on dr´astica de los zonotopos. 5. Conclusiones Se ha presentado la aplicaci´on sobre un quadrotor de dos estimadores garantistas, uno basado en aritm´etica intervalar y otro en el uso de zonotopos. La aplicaci´on de algoritmos garantistas se llev´o a cabo debido a que se tiene un modelo continuo con medidas del posici´on de forma discreta, en tiempos mayores a los de muestreo del sistema. Por tanto si solo se usara la informaci´on del sensor, se tendr´a durante instantes de tiempos sin cerrar el lazo de control. Para tener el lazo de control cerrado se tiene el modelo al que se le a˜naden posibles incertidumbres y se contempla los posibles estados a los que puede llegar el sistema. Adem´as, permite percatarse de que no solo se necesita el camino que se desee seguir libre de obst´aculos, sino que adem´as se necesita una zona donde es posible que se encuentre el quadrotor. Agradecimientos Los autores quieren expresar su agradecimiento al MCeI por la financiaci´on de este trabajo, a trav´es de los proyectos DPI2010-19154 y DPI201237580-C02-02 as´ı como a FAPEMIG y al Programa Institucional de Aux´ılio `a Pesquisa de Doutores Rec´emContratados de la PRPq/UFMG. Referencias [1] Abbott, E. and Powell, D., Land-Vehicle Navigation Using GPS. Proceedings of the IEEE, Vol. 87, No.1, 1999. [2] Alamo, T., Bravo, J.M. and Camacho,E.F. Guaranteed state estimation by zonotopes. Automatica Vol. 41, Issue 6, pages 1035-1043, 2005. [3] Bravo, J.M., Alamo, T. y Camacho, E.F., Estimaci´on Garantista de Estados en Sistemas H´ıbridos. [4] Bravo, J.M., Control predictivo no lineal robusto basado en t´ecnicas intervalares (Tesis Doctoral) [5] Brunke, S. and Campbell, M., Estimation Architecture for Future Autonomous Vehicles. Proceedings of the American Control Conference, 2002. [6] Combastel,C., A State Bounding Observer Based on Zonotopes. Proceedings of European Control Conference, 2003 [7] Combastel, C., A State Bounding Observer for Uncertain Non-linear Continuous-time Sstems based on Zonotopes. Proceedings of the 44th IEEE Conference on Decision and Control, and the European Control Conference, 2005. [8] Farrelly, J. and Wellstead, P., Estimation of Vehicle Lateral Velocity. Proceedings of the 1996 IEEE International Conference on Applications, 1996. [9] Jaulin, L., Nonlinear bounded-error state estimation of continuous-time systems. Automatica, Vol. 38, Issue 6, pages 1079-1082, 2002. [10] K¨uhn, W, Rigorously Computed Orbits of Dynamical Systems without the Wrapping Effect. Computing 61, 47-67, 1998. [11] Orihuela, L., Rubio, F.R. and G´omez-Estern, F., Model-Based Networked Control Systems under Parametric Uncertainties. 18th IEEE International Conference on Control Applications, 2009. [12] Ra¨ısi, T., Efimov, D. and Zolghadri, A., Interval State Estimation for a Class of Nonlinear Systems. IEEE Transactions on Automatic Control, Vol. 57, No.1, 2012. [13] Redondo, M.J., Aplicaciones de T´ecnicas DC a Identificaci´on Param´etrica, Estimaci´on de Estados y Conjuntos Invariantes, en Sistemas No Lineales (Tesis Doctoral) [14] Scholte, E. and Campbell, M.E., On-line Nonlinear Guaranteed Estimation with Application to a High Performance Aircraft. Proceedings of the American Control Conference, 2002. [15] Vu Tuan Hieu LE, Robust predictive control by zonotopic set-membership estimation (Tesis Doctoral) Figura 7: Zonotopo X ˙ Xcon GPS sin incertidumbres. Figura 8: Zonotopo X ˙ Xcon GPS con incertidumbres