efectos de la no linealidad del modelo...
TRANSCRIPT
EFECTOS DE LA NO LINEALIDAD DEL MODELO DINÁMICO EN EL CONTROL DE UN ROBOT SUBACUÁTICO
Daniel García, Ricardo Morales, Nicolás García, J.M. Azorín , J.M. Sabater, Carlos Pérez Virtual Reality & Robotics Lab Universidad Miguel Hernández
Av. Ferrocarril s/n Edificio Torreblanca 03200 Elche (Alicante) [email protected]
Resumen En este artículo se examina el modelado dinámico de un vehículo subacuático y que repercusión tiene el hecho de asumir un modelo simplificado frente a la no simplificación del mismo. Además, se presenta una herramienta software desarrollada en Matlab® Simulink® y en Guide® para el estudio del modelado y control de vehículos subacuáticos. Los resultados presentados en este artículo han sido obtenidos para un robot subacuático del tipo KAMBARA. Palabras Clave: Robótica Submarina, Control, Modelado, Simulación 1 INTRODUCCIÓN
La robótica subacuática ha despertado un gran interés en los últimos años, motivada por el afán del ser humano por la conquista del fondo marino. Este entorno subacuático es un ambiente hostil donde un robot puede aportar importantes ventajas, evitando riesgos a seres humanos y cumpliendo la operatividad de las misiones para las que fueron diseñados.
Los robots submarinos al igual que en otras aplicaciones, pueden operar de manera teleoperada (ROV) o de manera autónoma (AUV).
En el primer caso, el robot teleoperado a control remoto también llamado ROV, queda permanentemente unido, mediante un cable “cordón umbilical” a un barco de apoyo o a una plataforma. El cordón umbilical permite la transmisión de señales de control y de las medidas y observaciones captadas por los sensores dispuestos en el robot, la transmisión eléctrica para los sistemas y disponer de un cable que permita su recuperación mediante tracción. El operador desde su puesto de mando remoto controla los movimientos del robot, además de recibir las informaciones captadas por los sensores.
Los AUV’s por su parte pueden desarrollar una tarea preprogramada, interactuar en su entorno, cumplir con su objetivo y retornar a un lugar predesignado, sin intervención del operador. A diferencia de los ROV, no utiliza cable umbilical, precisando de un importante componente hardware y software destinado a su navegación totalmente autónoma.
En el estudio dinámico de un vehículo subacuático se suele asumir: el centro de gravedad como el origen del sistema de referencia rG = [0 0 0]T; los términos de la diagonal principal de la matriz de inercia prevalecen frente a los otros, está consideración siempre se dará si existe simetría en todos los planos del vehículo; y se considera parámetros de la diagonal principal de la matriz de masa añadida despreciándose los otros términos que la componen, debido a su complejidad de cálculo [1, 2, 3, 4].Estás consideraciones pueden dar lugar a errores a la hora de caracterizar y modelar el robot subacuático.
El objetivo del artículo es el estudio del efecto que produce el no considerar las condiciones anteriormente mencionadas en la dinámica y control de un robot subacuático. Las simulaciones que se han realizado concretando para un robot del tipo KAMBARA [1]. Éste robot consta de cinco motores, tiene 5 DOF, es un AUV (Vehículo autónomo subacuático) desarrollado en la Universidad Australiana [2].
En el artículo se presenta el software para el modelado y control de un robot subacuático implementado en la Universidad Miguel Hernández el cual permite simular a partir de las características y los parámetros del robot el comportamiento dinámico del vehículo.
El artículo esta estructurado en 6 secciones. En la sección 2, se expone la formulación de la dinámica y cinemática de un robot subacuático, así como los parámetros que intervienen. La cinemática del vehículo se formula en la sección 3. Posteriormente en la sección 4, se propone un control de seguimiento PD (Proporcional – Derivativo). La herramienta creada en Matlab/Simulink/Guide®, que incluye
ventanas de edición de parámetros para poder simular cualquier vehículo subacuático se presenta en la sección 5. En esta sección se muestran también los resultados de las simulaciones realizadas. 2 MODELADO DINÁMICO DE UN
VEHÍCULO SUBACUÁTICO La dinámica de un robot submarino se rige por la
siguiente ecuación:
(1) donde:
M ≡ Matriz de inercia. C ≡ Matriz de Coriolisis. D ≡ Matriz de amortiguamiento.
La matriz de inercia, M, tiene dos componentes,
por un lado la matriz de masa e inercia del cuerpo rígido, MRB, y por otro matriz de masa hidrodinámica añadida, MA. La matriz de inercia será la suma de dos matrices:
(2)
La matriz de coriolisis, C (v), también está
compuesta de dos componentes, por un lado la matriz de coriolisis y centrípeta del cuerpo rígido,
)(vCRB , y por otro la matriz de coriolisis de la
masa añadida, )(vCA .
(3)
2.1 MATRIZ DE MASA E INERCIA DEL CUERPO RÍGIDO
La matriz de masa e inercia define la masa del vehículo y la matriz de inercia del mismo.
(4) Siendo las coordenadas
del centro gravedad del vehículo y tomando la diagonal principal de la matriz de inercia, la matriz quedaría:
(5)
2.2 MASA AÑADIDA
Un fenómeno que afecta al vehículo subacuático es la masa añadida. Este fenómeno se atribuye debido a que el movimiento de un vehículo subacuático a través de un líquido produce que cierta cantidad de este líquido se mueva con él.
Esta matriz de masa añadida se calcula a partir de la geometría que tenga el vehículo, se considera difícil de calcular [3].
La matriz de masa añadida se define:
(6)
2.3 AMORTIGUAMIENTO HIDRODINÁMICO
Cuando un vehículo subacuático se está moviendo
a través del agua, se dan unas fuerzas que se oponen a la dirección del movimiento del vehículo, esta se denominan fuerzas de amortiguamiento hidrodinámico. La fuerza de amortiguamiento hidrodinámico se compone de dos términos: por un lado, los términos cuadráticos de la fuerza ascensional o de sustentación (DQ) y por otro lado, los términos lineales de la fuerza de fricción (DL) [4].
La fuerza de amortiguamiento hidrodinámico tiene un efecto significativo en la dinámica del vehículo que puede llegar producir una no lineabilidad. Se propone una simplificación en los parámetros del la fuerza de amortiguamiento hidrodinámico, consiste en tomar las diagonal principal de la matriz de términos lineales y la matriz de términos cuadrático.
(7)
Esta fuerza depende de la velocidad del vehículo.
{ }rNqMpKwZvYuXLD = (8)
{ }|||||||||||| rrNqqMppKwwZvvYuuXQD = (9)
(10)
ARB MMM +=
τη =+++ )()()()(.
gvvDvvCvM
)()()( vCvCvC ARB +=
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
−−−−−−−−−
−−
−
=⎥⎦
⎤⎢⎣
⎡ −=
IzzIzyIzxxmymIyzIyyIyxxmzmIxzIxyIxxymzm
xmymmxmzmmygmzmm
IrgSmrgSmxIm
M
gg
gg
gg
gg
gg
g
BRB
00
0000
000000
)()(33
[ ] [ ]TTgggg zyxr 0,0,0,, ≈=
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
=
IzzIyy
Ixxm
mm
MRB
000000000000000000000000000000
⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
−=
.....
.....
.....
.....
....
....
.
.
.
.
..
..
rNqNpNwNvNuN
rMqMpMwMvMuM
rKqKpKwKvKuK
rZqZpZwZvZuZ
rYqYpYwYvYuY
rXqXpXwXvXuX
MA
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
++
++
++
−=
rNNrqMMq
pKKpwZZw
vYYvuXXu
vD
rr
pp
ww
vv
uu
000000000000000000000000000000
)(
||VkDDD QL +=
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
+−−+++−−−++−
=
)()()()()()()()()(
000000000
qypxmurymvrxmuqzmpxrzmwqxmvpzmwpymrzqym
C
gggg
gggg
ggggRB
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
−+−+−+−−−+−++−−
++−−−−−++−+−−−+
00
0)()()(
)()()()()()(
pIqIrIqIpIrIpIqIrIrIpIqI
qIpIrIrIpIqIqypxmuqzmvqzm
urympxrzmwpymvrxmwqxmrzqym
xxxyxzyyxyyz
xxxyxzzzxzyz
yyxyyzzzxzyz
gggg
gggg
gggg
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
−−−−−
−−−
−−
=
0000
000000
00000000
pIqIumvmpIrIzzumwmqIrIzzvmwm
umvmumwmvmwm
C
xxyy
xx
yyRB
2.3 CORIOLIS Coriolis es una fuerza de inercia que actúa
perpendicular a la dirección de movimiento de un cuerpo. Esta fuerza es proporcional a la velocidad y a la rotación del sistema de coordenadas del vehículo con respecto el sistema de referencia.
Se compone como se ha dicho anteriormente, de la matriz de coriolis de masa añadida (Ca):
(11)
Siendo:
(12) Tomando la diagonal principal de la matriz de
masa añadida, la matriz de coriolis de masa añadida quedaría:
(13) Y la matriz de Coriolis de cuerpo rígido (Crb):
(14) Considerando , quedaría:
3 CINEMÁTICA DEL VEHÍCULO Nuestro objetivo es realizar un seguimiento
mediante ordenador de la posición y orientación del vehículo.
Para lograr ese propósito resulta imprescindible convertir los parámetros de estado en términos del sistema de referencia del robot {K} a parámetros del sistema de referencia del mundo real {W}. En particular se utiliza para convertir la velocidad lineal y la velocidad angular del sistema de referencia del robot (K) al sistema tomado como referencia externa (W) [3].
(15)
(16)
4 CONTROL DE SEGUIMIENTO PD
La tecnología de la robótica submarina esta en
continuo avance, al igual que las búsqueda del diseño e implementación del controlador, de manera que esté sea lo más exacto posible y lo más rápido en alcanzar el objetivo.
Una técnica de control muy utilizada por su fácil implantación es el control PID. Esta técnica de seguimiento PD ha sido muy utilizada en AUV’s, como por ejemplo, en KAMBARA por Silpa-Anan [2] o MAKO por L.A. Gonzalez [5], también se han implementado un simple controlador lineal cuadrático para el control de un AUV [6,7]. A pesar de la sencillez de estas técnicas, recientemente otras técnicas de control más complejas están siendo utilizadas en este campo de la robótica subacuática.
En este artículo se centra en control de seguimiento PD (Proporcional –Derivativo), cuyo esquema de control se muestra en la figura 1.
El propósito de este controlador es alcanzar una posición deseada a la salida del modelado dinámico y cinemática del vehículo del bloque AUV representado en la figura 1. Los bloques Kp y Kv representan los parámetros del controlador proporcional y derivativo respectivamente.
5 SOFTWARE Y SIMULACIONES El software creado consta de una serie de
aplicaciones creadas con la herramientas Guide®, cada una de ellas con una finalidad para el modelado y control de un vehículo subacuático (figura 2).
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
−−−−
−−−
−−
=
0000
000000
00000000
)(
1212
1313
2323
12
13
23
bbaabbaa
bbaaaa
aaaa
vC A
rNqMpKwZvYuXb
rMqMpKwZvYuXb
rKqKpKwZvYuXb
rZqZpZwZvYwXa
rYqYpYwYvYuXa
rXqXpXwXvXuXa
rrrrrr
rqqrqq
rqppPp
rqpwww
rqpwvv
rqpwvu
......3
......2
......1
......3
......2
......1
+++++=
+++++=
+++++=
+++++=
+++++=
+++++=
⎟⎟⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜⎜⎜
⎝
⎛
−−−−
−−−
−−
=
0000
000000
00000000
)(
....
....
....
..
..
..
pKqMuXvYpKrNuXwZ
qMrNvYwZuXvY
uXwZvYwZ
vC
Pquv
Pruw
qrvw
uv
uw
vw
A
[ ] [ ]TTgggg zyxr 0,0,0,, ≈=
')·,,( vRv ψθφ=
')·,,( wTw ψθφ=
Figura 1: Esquema de control de seguimiento PD (Proporcional –Derivativo).
Una aplicación para el modelado dinámico y
cinemática del vehículo subacuático figura 3, por defecto se muestran los parámetros del robot KAMBARA, pero es posible editarlos para simular otros vehículos subacuáticos en la aplicación que se muestra en la figura 4. Otra aplicación se ha creado para realizar el control de seguimiento PD anteriormente expuesto, figura 5.
Figura. 2. Pantalla principal del Software
desarrollado. Las aplicaciones de modelado dinámico y control
tienen un esquema Simulink, que permite la simulación, modelado y control de seguimiento PD (Proporcional –Derivativo) del robot subacuático, sometido a estudio por parte del usuario.
En las tablas: 1, 2, 3, se muestran las funciones de Matlab creadas, así como una breve descripción de la operatividad.
Figura. 3. Aplicación de Modelado dinámico y
cinemático.
Figura. 4. Aplicación de edición de parámetros del modelado dinámico.
Kp + AUV + M +
D (V) V +C(V) V+ G
+
Kv
Є
dx
dx.
dx..
P
V _-
+ +
+
+
+_
+
+
.∈
Figura. 5. Aplicación de control de seguimiento PD (Proporcional –Derivativo).
Tabla 1 Modelos Simulink creados En La Herramienta.
Función Simulink® Breve descripción modelo_dinamico_final.mdl Esquema en diagrama de
bloques para el modelado dinámico y cinemático del vehículo.
control_pid.mdl Esquema en diagrama de bloques para el control de un vehículo subacuático.
Tabla 2 Funciones de MATLAB creadas.
Función Matlab® Breve descripción euler2q.m Devuelve un vector de
cuatro términos, los ángulos de Euler y el cuarto término.
Rzyx.m Devuelve la matriz de rotación a partir de los ángulos de Euler.
Modelo_dina.m Proporciona la ejecución de la aplicación del modelado dinámico y la cinemática del vehículo.
editar_caracteristicas.m Proporciona la ejecución de la aplicación de edición de parámetros del modelo dinámico.
control_interfaz.m Proporciona la ejecución de la aplicación de control de seguimiento PD (Proporcional – Derivativo).
param_Ma.m Permite editar los parámetros de la masa añadida.
param_D.m Permite editar los parámetros de la matriz de damping.
Tabla 3 Funciones creadas en la herramienta GUIDE.
Función Guide® Breve descripción Modelo_dina.fig Proporciona la apariencia
de la aplicación del modelado dinámico y la cinemática.
editar_caracteristicas.fig Proporciona la apariencia de la aplicación de edición de parámetros.
control_interfaz.fig Proporciona la apariencia de la aplicación de control del vehículo.
param_Ma.fig Ventana de edición de la matriz de masa añadida.
param_D.fig Ventana de edición de la matriz de damping.
5.1 Simulaciones
Se han realizado diferentes simulaciones utilizando el caso concreto de un robot subacuático del tipo KAMBARA. Por un lado, se tomado el modelo dinámico asumiendo las consideraciones anteriormente expuestas en este artículo con el objetivo de simplificarlo y a continuación, se muestra el efecto de no linealizar el modelo.
En las figuras 6 y 7 se muestran los vectores de estado de posición y de velocidad considerando el cdg en distintas posiciones cuando una fuerza de 20 Newtons es aplicada a los motores 1 y 2 durante un tiempo de 10 segundos. En concreto se representan en color rojo el caso de que cdg este situado en la posición , en color verde el caso de que cdg este situado en la posición y en color azul el caso de que el cdg este situado en la posición utilizando el modelo simplificado.
En la figura 8 se muestra la posición en
coordenadas X-Y-Z, como resultado de simular el control de un robot subacuático de tipo KAMBARA, tomando como parámetros del controlador Kp = [0.1 0.025 0.05 0.1 0.1 0.1] y Kv = [0.6 0.4 0.4 0.6 0.6 0.6]. Los parámetros tomados en la simulación, es de un tiempo de simulación de 50 segundos y la posición que se desea alcanzar es [x=4, y=2, z= 0] a la salida del modelado dinámico y cinemático. Se representa en azul el modelo simplificado y en rojo el considerar el cdg posicionado en .
[ ]Tgr 000=
[ ]Tgr 02,002,002,0=[ ]Tgr 01,001,001,0=
[ ]Tgr 02,002,002,0=
0 1 2 3 4 5 6 7 8 9 100
1
2
3x
(m)
Vector de estado de la posicion
0 1 2 3 4 5 6 7 8 9 10-0.02
0
0.02
0.04
0.06
y (m
)
0 1 2 3 4 5 6 7 8 9 10-0.5
0
0.5
1
1.5
z (m
)
Tiempo de simulacion (seg)
0 1 2 3 4 5 6 7 8 9 10-0.06
-0.04
-0.02
0
0.02
Ang
ulo
de E
uler
ε1 (
rad)
Parametros de Euler
0 1 2 3 4 5 6 7 8 9 10-1.5
-1
-0.5
0
Ang
ulo
de E
uler
ε2 (
rad)
0 1 2 3 4 5 6 7 8 9 100
0.02
0.04
0.06
Ang
ulo
de E
uler
ε3 (
rad)
Tiempo de simulacion (seg)
Figura. 6. Resultados del vector de estado de
posición.
6 CONCLUSIONES Se han desarrollado unas herramientas que
permiten simular, modelar y controlar un vehículo subacuático y se ha particularizado a un robot de una estructura similar al KAMBARA.
También se ha constatado con los resultados de las simulaciones representados en las figuras 5, 6 y 7 se puede concluir que el hecho de no suponer la coordenada del centro de gravedad en el origen del sistema de referencia afecta muy poco a los vectores de posición y velocidad. Por tanto, la simplificación realizada por la mayoría de los investigadores en control de vehículos submarinos es aceptable siempre que se asuma el pequeño error que se produce.
Como se observa en la figura 8 la repercusión de esta simplificación en el control de seguimiento PD es también mínima. Estas simulaciones refrendan la consideración de que el centro de gravedad este situado en el origen del sistema de referencia del
vehículo para el modelado y control en seguimiento de un robot subacuático.
0 1 2 3 4 5 6 7 8 9 100
0.1
0.2
0.3
0.4
u (m
/seg
)
Vector de estado de velocidad
0 1 2 3 4 5 6 7 8 9 10-3
-2
-1
0x 10
-3
v (m
/seg
)
0 1 2 3 4 5 6 7 8 9 10-0.04
-0.03
-0.02
-0.01
0
w (
m/s
eg)
Tiempo de simulacion (seg)
0 1 2 3 4 5 6 7 8 9 10-6
-4
-2
0
2x 10
-3
p (r
ad/s
eg)
Vector de estado de velocidad angular
0 1 2 3 4 5 6 7 8 9 10-0.2
-0.15
-0.1
-0.05
0
q (r
ad/s
eg)
0 1 2 3 4 5 6 7 8 9 100
0.005
0.01
0.015
r (r
ad/s
eg)
Tiempo de simulacion (seg)
Figura. 7. Resultados del vector de estado de velocidad.
0 5 10 15 20 25 30 35 40 45 500
2
4
6
x (m
)
Vector de estado de la posicion
0 5 10 15 20 25 30 35 40 45 500
0.5
1
1.5
2
0 5 10 15 20 25 30 35 40 45 50-5
0
5
10
15x 10
-17
z (m
)
Tiempo de simulacion (seg)
Figura 8. Resultado de salida del control de seguimiento PD.
Referencias [1] Página web del robot KAMBARA, por la
universidad Nacinal Australiana (ANU), del departamento de ingeniería y ciencia. [Online]. Habilitable http://users.rsise.anu.edu.au/~kambara/.
[2] Chanop Silpa-Anan. Autonomous Underwater Robot: Vision and control. Master’s thesis. Australian National University, February 2001.
[3] Ola-Erik Fjellstad. Control of Unmanned Underwater Vehicles in Six Degrees of freedom. A quaternion Feedback Approach. Univesity of Trondheim, Norway. November, 1994.
[4] Oliver Scout. Computation of hydrodynamic coeficients and determination of dynamic stability chacteritics of an underwater vehicle including free surface effects. Thesis. Florida Atlantic University, May 2003.
[5] Louis Andrew Gonzalez. Design, Modelling and Control of an Autonomous underwater vehicle. Thesis. University of Western Australia, 2004.
[6] W. Naeem, R. Sutton, S.M. Ahmad. LQG/ LTR Control of an Autonomous Underwater Vehicle using a Hybrid Guidance Law. International Federation on Automatic Control Journal, 2003.
[7] Pramod Maurya, E. Desa, A. Pascoal, E. Barros, G. Navelkar, R Madhan, A. Mascarenhas , S. Prabhudesai, S.Afzulpurkar, A. Gouveia, S. Naroji, L.Sebastiao. Control of the maya auv in the vertical and horizontal planes: theory and practical results. National Institute of Oceanography Goa, India.