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) nicolas.garcia@umh.es

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

qq

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.