Modelo dinámico y control del cuadricóptero.

Modelo dinámico de un cuadricóptero

El cuadricóptero es un aeronave de despegue y aterrizaje vertical que utiliza 4 motores eléctricos en su sistema de propulsión. La Figura 1 muestra un cuadricóptero típico.

Figure 1: Cuadricóptero

Para compensar los momentos de rotación alrededor del eje vertical, los motores cuadricóptero giran en diferentes direcciones. Como se puede ver en la Figura 2, un motor que gira en sentido horario tiene dos motores adyacentes que giran en sentido antihorario.

Figure 2: Cuadricóptero. Motores

Los esquemas de las dos configuraciones de cuadricóptero más comunes “+” y “X” se muestran en la Figura 3.

Figure 3: Los configuraciones “+” y “X” de cuadricóptero

Utilizamos las ecuaciones de dinámica de un cuerpo rígido (ecu. 1) obtenidas en el capitulo anterior para construir un modelo dinámico de un cuadricóptero. El sistema de coordenadas, la configuración del cuadricóptero y el sentido de rotación de las hélices se elegirán como se indica en (Beard, 2008). Para completar el modelo dinámico, necesitamos encontrar las fuerzas que actúan sobre el cuadricóptero $F^b$ , los momentos $m^b$  y la matriz de inercia $J$ .

$\{\begin{array}{c}\left(\begin{array}{c}{\dot{P}}_N\\ {\dot{P}}_E\\ \dot{P_D}\end{array}\right)=R_b^v\left(\begin{array}{c}u\\ v\\ w\end{array}\right)\\ \left(\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right)=\left(\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi \mathit{sec}\theta & \cos \varphi \mathit{sec}\theta \end{array}\right)\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\\ \left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)=\frac{F^b}{m}-\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\times \left(\begin{array}{c}u\\ v\\ w\end{array}\right)\\ \left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)=J^{-1}\left(m^b-\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\times J\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\right)\end{array}$   (1)

Fuerzas

La fuerza total $F^b$  que actuá sobre el aeronave en el marco de referencia del cuerpo es

$F^b=F_g^b+F_a^b+F_p^b$  

donde Fg es fuerza de gravedad, Fa es la fuerza aerodinámica, y Fp es la fuerza de propulsión.

Como se muestra en la Figura 4, cada hélice produce una fuerza $F_i=-k_1{\delta }_i$  en dirección de $-k^b$ . El coeficiente $k_1$  es una constante asociada al motor, determinada experimentalmente y ${\delta }_i={\omega }_i^2$  es el variable de control, que es proporcional al cuadrado de la velocidad angular del i-esimo motor.

Figure 4: Cuadricóptero de configuración “+”. Fuerzas y momentos.

El indice $i=1,2,3,4$  representa las posiciones de los motores (front, right, back, left). Los motores 1 y 3 giran en sentido de las manecillas de reloj, mientras que los motores 2 y 4 en contra de las manecillas del reloj.

La fuerza de propulsión total, producida por los motores, que actuá sobre un cuadricóptero se define como

$F_p^b=\left(\begin{array}{c}0\\ 0\\ -\sum _{i=1}^4{k}_1{\delta }_i\end{array}\right)$ ,

Las fuerzas aerodinámicas estarán determinadas únicamente por las fuerzas de resistencia del aire como esta definido en (Luukkonen, 2011).

$F_a^b=-\left(\begin{array}{ccc}{A}_x& 0& 0\\ 0& A_y& 0\\ 0& 0& A_z\end{array}\right)\left(\begin{array}{c}u\\ v\\ w\end{array}\right)$ .

donde Ax, Ay y Az son los coeficientes de fuerza de arrastre para velocidades en las direcciones correspondientes del marco de cuerpo. El signo menos aparece porque la fuerza de resistencia al aire siempre se dirige en sentido opuesto a la velocidad de movimiento del cuerpo.

La fuerza de gravedad en el marco del vehículo es

$F_g^v=\left(\begin{array}{c}0\\ 0\\ mg\end{array}\right)$ ,

donde m es masa de cuadricóptero y g es aceleración de gravedad.

Los componentes de la fuerza gravitacional en el marco del cuerpo son

$F_g^b=R_v^b\left(\begin{array}{c}0\\ 0\\ mg\end{array}\right)=\left(\begin{array}{c}-mg\sin \theta \\ mg\cos \theta \sin \varphi \\ mg\cos \theta \cos \varphi \end{array}\right)$  

Finalmente, la fuerza total es

$F^b=R_v^b\left(\begin{array}{c}0\\ 0\\ mg\end{array}\right)+\left(\begin{array}{c}0\\ 0\\ F_p\end{array}\right)-\left(\begin{array}{ccc}{A}_x& 0& 0\\ 0& A_y& 0\\ 0& 0& A_z\end{array}\right)\left(\begin{array}{c}u\\ v\\ w\end{array}\right)$ . (2)

Momentos

El momento total que actúa sobre la aeronave en el marco de referencia del cuerpo es

$m=m_a+m_p$ ,

donde $m_a$  y $m_p$  son los momentos debido a la aerodinámica y propulsión.

Suponemos que los momentos aerodinámicos son $m_a=0$ .

Debido a la tercera ley de Newton, el arrastre de la hélice produce un torque de guiñada al cuerpo del cuadricóptero ${\tau }_i=k_2{\delta }_i$ , donde $k_2$  es una constante asociada al motor y es determinada experimentalmente. La dirección del torque sera en dirección opuesta a la dirección de giro de la hélice. De este modo, el torque total de guiñada esta dado por

${\tau }_{\psi }={\tau }_2+{\tau }_4-{\tau }_1-{\tau }_3$ .

Para la configuración “+” que se muestra en la Figura 4 el torque de alabeo es producido por las fuerzas de los motores derecho e izquierdo y se define como

${\tau }_{\varphi }=L(F_4-F_2)$ ,

donde L es distancia entre el centro de cuadricóptero y el centro de un motor.

El torque de cabeceo es un producto de las fuerzas de los motores de enfrente y atrás

${\tau }_{\theta }=L(F_1-F_3)$ .

Figure 5: Cuadricóptero de configuración “X”. Fuerzas y momentos.

Para la configuración “X” que se muestra en la Figura 5 el torque de alabeo es

${\tau }_{\varphi }=\frac{\sqrt{2}}{2}L(F_1-F_2-F_3+F_4)$ .

El torque de cabeceo es

${\tau }_{\theta }=\frac{\sqrt{2}}{2}L(F_1+F_2-F_3-F_4)$ .

Finalmente, los componentes de los momentos en el marco de cuerpo son

$m^b=\left(\begin{array}{c}{\tau }_{\varphi }\\ {\tau }_{\theta }\\ {\tau }_{\psi }\end{array}\right)$ . (3)

La matriz de inercia

La matriz de inercia caracteriza la rotación inercial de un cuerpo rígido, y esta dada por la siguiente expresión

$J=\left(\begin{array}{ccc}\int (y^2+z^2)\mathit{dm}& -\int xy\mathit{dm}& -\int xz\mathit{dm}\\ -\int xy\mathit{dm}& \int (x^2+z^2)\mathit{dm}& -\int yz\mathit{dm}\\ -\int xz\mathit{dm}& -\int yz\mathit{dm}& \int (x^2+z^2)\mathit{dm}\end{array}\right)$ ,

$J\equiv \left(\begin{array}{ccc}{J}_{\mathit{xx}}& -J_{\mathit{xy}}& -J_{\mathit{xz}}\\ -J_{\mathit{yx}}& J_{\mathit{yy}}& -J_{\mathit{yz}}\\ -J_{\mathit{zx}}& -J_{\mathit{zy}}& J_{\mathit{zz}}\end{array}\right)$ .

El cuadricóptero es simétrico sobre sus ejes, lo que implica que

$J=\left(\begin{array}{ccc}{J}_{\mathit{xx}}& 0& 0\\ 0& J_{\mathit{yy}}& 0\\ 0& 0& J_{\mathit{zz}}\end{array}\right)$ . (4)

Finalmente, ecuaciones (1), (2), (3) y (4) definen el modelo dinámico de seis grados de libertad de un cuadricóptero.

Validación de modelo dinámico.

Para validar el modelo dinámico desarrollado, se utilizan los resultados de la simulación realizada por (Luukkonen, 2011). En este trabajo se muestra la respuesta de un modelo dinámico de un cuadricóptero a una variación de las señales de control (voltajes en los motores del cuadricóptero). Los cambios en las señales de control en el tiempo, utilizados en (Luukkonen, 2011), se muestran en la Figura 6.

Figure 6: Señales de control.

Como se puede ver en la Figura 6, durante el primer período (los primeros 0.5 segundos), el voltaje cambia por igual en los cuatro motores. El resultado de esta maniobra es un cambio en la altitud del cuadricóptero. Los ángulos de actitud del cuadricóptero en esta maniobra no cambian.

Un aumento de voltaje en el motor 4 y una disminución en el motor 2, al comienzo del segundo período (0.5 - 0.75 segundos), provoca un alabeo positivo. El voltaje en los motores 1 y 3 permanece sin cambios durante el segundo período.

Al comienzo del tercer período (1.0-1.25 segundos), el cabeceo negativo se logra al disminuir el voltaje en motor 1 y aumentar en el motor 3. El voltaje en los motores 2 y 4 permanece sin cambios durante el tercer período.

Al comienzo del cuarto período (1.5-1.75 segundos), el cuadricóptero realiza la guiñada debido a un aumento simultáneo de voltaje en los motores 2 y 4 y una disminución en los motores 1 y 3.

Las curvas presentadas en la Figura 6 se muestrearon (discretizaron en el tiempo) con un paso de 0.01 segundos, y se guardaron en el archivo de texto commands.txt como una tabla de comandos. El formato de la tabla de comandos es el siguiente. Cada evento es un conjunto de 5 números: el tiempo absoluto del evento y 4 valores de las señales de control (cuadrados de voltajes de los motores del cuadricóptero). Se muestra un ejemplo en la Tabla 1.

Tabla 1: Ejemplo de una tabla de comandos

El programa del simulador desarrollado lee el archivo commands.txt antes de iniciar la simulación, y durante la simulación actualiza los valores de las señales de control (cuadrados de voltajes de los motores) de acuerdo con los valores presentados en la tabla de comandos.

Los parámetros del modelo dinámico presentados en la Tabla 2 se seleccionaron de manera idéntica a (Luukkonen, 2011)

Tabla 2: Parámetros del modelo dinámico

Los resultados de la simulación se muestran en las Figuras 7 y 8. Como se observa, los resultados de la simulación repiten con bastante precisión los resultados obtenidos en (Luukkonen, 2011).

De acuerdo con la Figura 3 de (Luukkonen, 2011) a 2 segundos de vuelo, el cuadricóptero estará en el punto x=1.0278, y=-2.48, z=-0.60. Obtendremos la misma posición como resultado de integración de nuestro modelo, como se muestra en la Figura 7.

Figure 7: Posición de cuadricóptero (m).

Figure 8: Actitud de cuadricóptero (grad).

Como puede ver en la Figura 4 del artículo (Luukkonen, 2011), después de dos segundos de simulación, el cuadricóptero estará en la actitud determinada por los ángulos ϕ=24.578, θ=23.856, ψ=6.080. La Figura 8 muestra que los resultados de la simulación para ambos modelos son idénticos. Con estos resultado se presume que modelo dinámico paso la validación.

Controlador de altura y actitud

Nuestro objetivo es desarrollar un controlador que permita al cuadricóptero flotar a cierta altura. Para ello utilizaremos el controlador PD (Ogata & Yang, 2002). La salida de controlador es proporcional al error $e(t)$  de modo que

$u(t)=K_{p}e(t)+K_d\frac{d}{\mathit{dt}}e(t)$ , (5)

donde $K_p$  y $K_d$  son los coeficientes de ganancia de los términos proporcional y derivativo.

El error se define como

$e(t)=x^{\text{*}}-x(t)$ , (6)

donde $x^{\text{*}}$  corresponde al valor deseado, y $x(t)$  al valor medido.

Para el diseño de un sistema de control, las formulas del modelo dinámico de un cuadricóptero (1), (2), (3) y (4) no son adecuadas porque son demasiado complicadas para obtener información importante sobre el movimiento de la aeronave (Beard, 2008). Por lo tanto, vamos a desarrollamos un modelo dinámico simplificado de un cuadricóptero.

Modelo dinámico para control

Queremos que el cuadricóptero este flotando horizontalmente por lo tanto ${\varphi }^{\text{*}}={\theta }^{\text{*}}=0$ . Asumiendo que $\varphi$  y $\theta$  son pequeños, podemos decir que $\sin \varphi =0$ , $\sin \theta =0$ , $\cos \varphi =1$  y $\cos \theta =1$ . Por lo que, la ecuación 2 del sistema (1) puede reescribirse como

$\left(\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right)=\left(\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi \mathit{sec}\theta & \cos \varphi \mathit{sec}\theta \end{array}\right)\left(\begin{array}{c}p\\ q\\ r\end{array}\right)$ => $\left(\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}p\\ q\\ r\end{array}\right)$ (7)

Simplificamos la cuarta ecuación del sistema (1).

$\left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)=J^{-1}{m}^b-J^{-1}\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\times J\left(\begin{array}{c}p\\ q\\ r\end{array}\right)$ => $\left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)=J^{-1}{m}^b-\left(\begin{array}{c}\frac{p}{J_{\mathit{xx}}}\\ \frac{q}{J_{\mathit{yy}}}\\ \frac{r}{J_{\mathit{zz}}}\end{array}\right)\times \left(\begin{array}{c}{J}_{\mathit{xx}}p\\ J_{\mathit{yy}}q\\ J_{\mathit{zz}}r\end{array}\right)$ => $\left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)=J^{-1}{m}^b-\left(\begin{array}{c}qr\left(\frac{J_{\mathit{zz}}}{J_{\mathit{yy}}}-\frac{J_{\mathit{yy}}}{J_{\mathit{zz}}}\right)\\ rp\left(\frac{J_{\mathit{xx}}}{J_{\mathit{zz}}}-\frac{J_{\mathit{zz}}}{J_{\mathit{xx}}}\right)\\ pq\left(\frac{J_{\mathit{yy}}}{J_{\mathit{xx}}}-\frac{J_{\mathit{xx}}}{J_{\mathit{yy}}}\right)\end{array}\right)$

Debido a que los términos de Coriolis qr, pr y pq son muy pequeños, finalmente tenemos

$\left(\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right)=J^{-1}{m}^b$   (8)

Derivando la ecuación (7) y combinándola con la (8) se obtiene

$\left(\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right)=J^{-1}{m}^b=J^{-1}\left(\begin{array}{c}{\tau }_{\varphi }\\ {\tau }_{\theta }\\ {\tau }_{\psi }\end{array}\right)$   (9)

Derivando la ecuación 1 del sistema (1) recibimos

$\left(\begin{array}{c}{\ddot{P}}_N\\ {\ddot{P}}_E\\ {\ddot{P}}_D\end{array}\right)={\dot{R}}_b^v\left(\begin{array}{c}u\\ v\\ w\end{array}\right)+R_b^v\left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)$ .

Ignorando la derivada ${\dot{R}}_b^v$ , obtenemos

$\left(\begin{array}{c}{\ddot{P}}_N\\ {\ddot{P}}_E\\ {\ddot{P}}_D\end{array}\right)=R_b^v\left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)$   (10)

Omitiendo los términos de Coriolis en la tercera ecuación del sistema (1), recibimos

$\left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)=\frac{F^b}{m}-\left(\begin{array}{c}p\\ q\\ r\end{array}\right)\times \left(\begin{array}{c}u\\ v\\ w\end{array}\right)$ => $\left(\begin{array}{c}\dot{u}\\ \dot{v}\\ \dot{w}\end{array}\right)=\frac{F^b}{m}$

Usamos (10). Recibimos

$\left(\begin{array}{c}{\ddot{P}}_N\\ {\ddot{P}}_E\\ {\ddot{P}}_D\end{array}\right)=R_b^v\frac{F^b}{m}$   (11)

o

$\left(\begin{array}{c}{\ddot{P}}_N\\ {\ddot{P}}_E\\ {\ddot{P}}_D\end{array}\right)=\left(\begin{array}{c}0\\ 0\\ g\end{array}\right)+R_b^v\left(\begin{array}{c}0\\ 0\\ F_p/m\end{array}\right)$   (12)

Por lo tanto, el modelo inercial simplificado esta presentado por ecuaciones (9) y (12). La dinámica dada por estas ecuaciones se expresa en el marco inercial.

Controlador PD

Desarrollaremos un controlador PD para la estabilización horizontal $(\varphi =\mathrm{0,}\theta =0)$  de un cuadricóptero a una altitud determinada $h^{\text{*}}=-P_d^{\text{*}}$ . El controlador contendrá 3 canales: de altura, de angulo alabeo $\varphi$  y de angulo cabeceo $\theta$ . El diagrama de bloques del controlador PD se muestra en la Figura 9.

Figure 9: El diagrama de bloques del controlador PD.

Consideremos el funcionamiento del canal de control de altura con más detalle. Para simplificar, asumiremos que solo tenemos un término proporcional en la señal de control, es decir tenemos controlador P. Como puede ver en la Figura 9, la salida del controlador $u(t)$  es proporcional al error de altura $e_z=P_D^{\text{*}}-P_D$ .

Asumiendo que la altitud actual $P_D=0$ . Queremos subir a $P_D^{\text{*}}=-1$  metros. Entonces el error es negativo $e_z=-1-0=-1$ . Pero la señal de control debe ser positiva porque queremos aumentar altura y, por eso, se debe aumentarse el voltaje en los motores. Por lo tanto, definimos que la señal de la salida del controlador de altura es

$u_z(t)={\delta }_z=-\frac{m}{4}{K}_{p,z}{e}_z$ , (13)

donde $K_{p,z}$  es la ganancia proporcional de canal de control de altura y m es masa de cuadricóptero. Recuerde, dado que tenemos 4 motores, podemos suponer que cada motor eleva 1/4 de la masa del cuadricóptero.

Por otro lado, como definimos anteriormente, la fuerza de 4 hélices del cuadricóptero en marco de cuerpo es $F_p^b=-4k_1{\delta }_i$   (14)

Sustituyendo (13) en (14) recibimos

$F_p^b=k_{1}m{K}_{p,z}{e}_z$   (15)

Para simplificar la notación, conviene introducir ahora una nueva variable ${\mathit{Ke}}_z$ . La variable ${\mathit{Ke}}_z$ significará ${\mathit{Ke}}_z=K_{p,z}{e}_z$ , ${\mathit{Ke}}_z=K_{p,z}{e}_z+K_{d,z}\frac{d{e}_z}{dt}$  y ${\mathit{Ke}}_z=K_{p,z}{e}_z+K_{i,z}\underset{0}{\overset{t}{\int }}{e}_z(\tau )d\tau +K_{d,z}\frac{d{e}_z}{dt}$  en el caso de P, PD y PID controladores, respectivamente. Aquí los coeficientes $K_{d,z}$  y $K_{i,z}$  son las ganancias derivativa e integral del canal de control de altura.

Entonces reescribimos la fuerza de propulsión (15) como

$F_p^b=k_{1}m{\mathit{Ke}}_z$   (16)

la fuerza de propulsión en marco inercial es

$F_p^i=\cos \varphi \cos \theta F_p^b$   (17)

De acuerdo a la segunda ley de Newton

$mg+F_p^i=m\ddot{P_D}$  o

$mg+\cos \varphi \cos \theta F_p^b=m\ddot{P_D}$ . (18)

Despejamos la fuerza

$F_p^b=\frac{-m(g-\ddot{P_d})}{\cos \varphi \cos \theta }$   (19)

Definimos que $\ddot{P_D}=k_1{\mathit{Ke}}_z$   (20)

Sustituimos (20) a (19) recibimos

$-k_1({\delta }_1+{\delta }_2+{\delta }_3+{\delta }_4)=\frac{-m(g-k_1{\mathit{Ke}}_z)}{\cos \varphi \cos \theta }$   (21)

Para desarrollar los canales del controlador PD de estabilización horizontal, introducimos las variables ${\mathit{Ke}}_{\varphi }$  y ${\mathit{Ke}}_{\theta }$ . Como en el caso de ${\mathit{Ke}}_z$ , estas variables sirven para facilitar la escritura de ecuaciones para diferentes tipos de controladores (P, PD y PID). En particular, para el controlador PD tenemos: ${\mathit{Ke}}_{\varphi }=K_{p,\varphi }{e}_{\varphi }+K_{d,\varphi }\frac{d{e}_{\varphi }}{dt}$  y ${\mathit{Ke}}_{\theta }=K_{p,\theta }{e}_{\theta }+K_{d,\theta }\frac{d{e}_{\theta }}{dt}$ ,

donde $e_{\varphi }={\varphi }^{\text{*}}-\varphi$  y $e_{\theta }={\theta }^{\text{*}}-\theta$  son los errores de alabeo y cabeceo. Los coeficientes $K_{p,\varphi }$  y $K_{d,\varphi }$  son las ganancias proporcional y derivativa de canal de control de alabeo, y los coeficientes $K_{p,\theta }$  y $K_{d,\theta }$  son las ganancias proporcional y derivativa de canal de control de cabeceo, respectivamente.

Del modelo dinámico tenemos

$\{\begin{array}{c}{\tau }_{\varphi }=L{k}_1({\delta }_4-{\delta }_2)\\ {\tau }_{\theta }=L{k}_1({\delta }_1-{\delta }_3)\\ {\tau }_{\psi }=k_2({\delta }_2+{\delta }_4-{\delta }_1-{\delta }_3)\end{array}$   (22)

Del modelo para el control tenemos

$\{\begin{array}{c}{\tau }_{\varphi }=J_{\mathit{xx}}\ddot{\varphi }\\ {\tau }_{\theta }=J_{\mathit{yy}}\ddot{\theta }\\ {\tau }_{\psi }=J_{\mathit{zz}}\ddot{\psi }\end{array}$   (23)

Igualamos (22) con (23) y tenemos

$\{\begin{array}{c}{J}_{\mathit{xx}}\ddot{\varphi }=L{k}_1({\delta }_4-{\delta }_2)\\ J_{\mathit{yy}}\ddot{\theta }=L{k}_1({\delta }_1-{\delta }_3)\\ J_{\mathit{zz}}\ddot{\psi }=k_2({\delta }_2+{\delta }_4-{\delta }_1-{\delta }_3)\end{array}$   (24)

Juntamos (21) con (24). Tenemos

$\{\begin{array}{c}{k}_1({\delta }_1+{\delta }_2+{\delta }_3+{\delta }_4)=\frac{m(g-k_1{\mathit{Ke}}_z)}{\cos \varphi \cos \theta }\\ J_{\mathit{xx}}\ddot{\varphi }=L{k}_1({\delta }_4-{\delta }_2)\\ J_{\mathit{yy}}\ddot{\theta }=L{k}_1({\delta }_1-{\delta }_3)\\ J_{\mathit{zz}}\ddot{\psi }=k_2({\delta }_2+{\delta }_4-{\delta }_1-{\delta }_3)\end{array}$   (25)

Definimos que $\ddot{\varphi }={\mathit{Ke}}_{\varphi }$ , $\ddot{\theta }={\mathit{Ke}}_{\theta }$ , $\ddot{\psi }={\mathit{Ke}}_{\psi }$   (26)

Finalmente, obtenemos el siguiente sistema de ecuaciones

$\{\begin{array}{c}{k}_1({\delta }_1+{\delta }_2+{\delta }_3+{\delta }_4)=\frac{m(g-k_1{\mathit{Ke}}_z)}{\cos \varphi \cos \theta }\\ J_{\mathit{xx}}{\mathit{Ke}}_{\varphi }=L{k}_1({\delta }_4-{\delta }_2)\\ J_{\mathit{yy}}{\mathit{Ke}}_{\theta }=L{k}_1({\delta }_1-{\delta }_3)\\ J_{\mathit{zz}}{\mathit{Ke}}_{\psi }=k_2({\delta }_2+{\delta }_4-{\delta }_1-{\delta }_3)\end{array}$   (27)

La solución de este sistema (señales a los motores del cuadricóptero) es

${\delta }_1=\frac{m(g-{\mathit{ke}}_z)}{4k_1\cos \varphi \cos \theta }+\frac{J_{\mathit{yy}}{\mathit{ke}}_{\theta }}{2L{k}_1}-\frac{J_{\mathit{zz}}{\mathit{ke}}_{\psi }}{4k_2}$
${\delta }_2=\frac{m(g-{\mathit{ke}}_z)}{4k_1\cos \varphi \cos \theta }-\frac{J_{\mathit{xx}}{\mathit{ke}}_{\varphi }}{2L{k}_1}+\frac{J_{\mathit{zz}}{\mathit{ke}}_{\psi }}{4k_2}$ (28)
${\delta }_3=\frac{m(g-{\mathit{ke}}_z)}{4k_1\cos \varphi \cos \theta }-\frac{J_{\mathit{yy}}{\mathit{ke}}_{\theta }}{2L{k}_1}-\frac{J_{\mathit{zz}}{\mathit{ke}}_{\psi }}{4k_2}$
${\delta }_4=\frac{m(g-{\mathit{ke}}_z)}{4k_1\cos \varphi \cos \theta }+\frac{J_{\mathit{xx}}{\mathit{ke}}_{\mathit{phi}}}{2L{k}_1}+\frac{J_{\mathit{zz}}{\mathit{ke}}_{\psi }}{4k_2}$

Validación de controlador PD.

Para la validación del controlador PD realizamos una comparación de los resultados de la simulación de estabilización de cuadricóptero mostrados en el artículo (Luukkonen, 2011) con los resultados de la simulación del nuestro modelo.

Usamos los mismos coeficientes del controlador, valores iniciales de modelo y valores deseables como se usaron en el articulo de Luukkonen. Los parámetros del controlador PD se presentan en la Tabla 3. La condición inicial del cuadcóptero para la posición es [0 0 1] en metros, y para la actitud es [10 10 10] en grados. El propósito de la estabilización es un vuelo estacionario estable con la altitud deseada $z^{\text{*}}=0$  y la actitud deseada [0 0 0].

Table 3: Parámetros del controlador PD

Los errores de posicionamiento en nuestro simulador se calculan de la siguiente manera:

$e_{p,z}=z^{\text{*}}-z$ , $\frac{d{e}_z}{dt}=e_{d,z}={\dot{z}}^{\text{*}}-\dot{z}$ , $e_{p,\varphi }={\varphi }^{\text{*}}-\varphi$ , $\frac{d{e}_{\varphi }}{dt}=e_{d,\varphi }={\dot{\varphi }}^{\text{*}}-\dot{\varphi }$ , $e_{p,\theta }={\theta }^{\text{*}}-\theta$ , $\frac{d{e}_{\theta }}{dt}=e_{d,\theta }={\dot{\theta }}^{\text{*}}-\dot{\theta }$ , $e_{p,\psi }={\psi }^{\text{*}}-\psi$ , $\frac{d{e}_{\psi }}{dt}=e_{d,\psi }={\dot{\psi }}^{\text{*}}-\dot{\psi }$ ,

donde z, $\dot{z}$ , $\varphi$ , $\dot{\varphi }$ , $\theta$ , $\dot{\theta }$ , $\psi$ , $\dot{\psi }$  provienen de resultado de la integración del modelo dinámico. Los valores deseados son ${\varphi }^{\text{*}}=0$ , $\dot{{\varphi }^{\text{*}}}=0$ , ${\theta }^{\text{*}}=0$ , $\dot{{\theta }^{\text{*}}}=0$ , ${\psi }^{\text{*}}=0$ , $\dot{{\psi }^{\text{*}}}=0$ .

Dado que nuestro modelo usa el sistema de coordenadas NED, y en el artículo de Luukkonen esta usando el sistema XYZ, para obtener imágenes similares a figuras 6, 7 y 8 los valores iniciales de la posición del cuadricóptero deben cambiarse a [0 0 -1] para posición y a [10 -10-10] para actitud.

Los resultados de la simulación de la estabilización del cuadricóptero se muestran en las figuras 10, 11 y 12. Las figuras 10, 11, que muestran señales de control y ángulos de actitud de cuadricóptero, son idénticas a las figuras 5 y 7 del artículo.

La Figura 12 muestra las curvas de cambios de la posición del cuadricóptero a lo largo del tiempo. Las curvas con índice 0 (x0, y0, z0) son datos del artículo de Luukkonen, las curvas x1, y1, z1 son el resultado de la simulación de nuestro modelo con el controlador PD. Como se puede ver en la figura, las curvas x y z coinciden hasta el error de discretización de las figuras del artículo. En cuanto a la curva y, el error relativo alcanza el 27% a los 6 segundos de simulación. No sabemos qué causó este error al calcular la coordenada y. Quizás el error se deba a una diferencia en la implementación de nuestro programa y el programa de Luukkonen. Por ejemplo, a una diferencia en el orden de integración de ecuaciones o el método para calcular los ángulos de posición.

Dado que la coordenada y no está controlada (solo se controlan los ángulos de actitud y la coordenada z), creemos que el modelo de controlador ha pasado la validación con éxito.

Figure 10: Señales de control.

Figure 11: Actitud de cuadricóptero (grad).

Figure 12: Posición de cuadricóptero (m).

Literatura

Beard, R. W. (2008). Quadrotor dynamics and control. Brigham Young University, 19(3), 46-56.

Luukkonen, T. (2011). Modelling and control of quadcopter. Independent research project in applied mathematics, Espoo, 22.

Ogata, K., & Yang, Y. (2002). Modern control engineering (Vol. 4). India: Prentice hall.