Respuesta oscilatoria en ecuacion de movimiento
Publicado por Arely (1 intervención) el 03/07/2023 11:55:36
Hola, espero me puedan ayudar, estoy resolviendo la ecuacion de movimiento de un robot submarino, por medio del metodo de euler, M(v')+ (D(v)v)+ g= tau, donde g es el vector de restitucion el cual si le doy valores a theta y phi, sin tener valores en tau (fuerza de arranque) me tiene que dar una grafica de movimiento oscilatorio, de tal forma que se va estabilizando el sistema hasta cero, No logro obtener esa respuesta, no se que deba modificar en mi codigo, les agradeceria mucho.
Como dato utilizo matrices de 6x6 en la M y D, g y tau son vectores de 6x1
Como dato utilizo matrices de 6x6 en la M y D, g y tau son vectores de 6x1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
% Condiciones iniciales<h = 0.1; % Tamaño del paso
t = 0:h:10; % Intervalo de tiempo
theta = deg2rad(0); % Pitch inicial
phi = deg2rad(5); % Roll inicial
% Parámetros para el movimiento oscilatorioamplitud_theta = deg2rad(10); % Amplitud del movimiento oscilatorio para theta (en radianes)
amplitud_phi = deg2rad(20); % Amplitud del movimiento oscilatorio para phi (en radianes)
frecuencia_theta =35;% 1; % Frecuencia del movimiento oscilatorio para theta (en Hz)
frecuencia_phi =45;% 2; % Frecuencia del movimiento oscilatorio para phi (en Hz)
% Matrices para almacenar la evolución de posición, velocidad y aceleracióna = zeros(6, length(t));
v0 = [0; 0; 0; 0; 0; 0];%zeros(6, 1) % Velocidad inicial
p0 = [0; 0; 0; 0; 0; 0];%zeros(6, 1); % Posición inicial
v(:,1) = v0;
p(:,1) = p0;
% Función de aceleraciónacc = @(v) inv(M) * (tau - dv * v - g_deg);
% Euler mejoradofor i = 1:length(t)-1
% Actualizar theta y phi en cada paso del tiempotime = t(i+1);
theta = amplitud_theta * sin(2 * pi * frecuencia_theta * time);
phi = amplitud_phi * sin(2 * pi * frecuencia_phi * time);
% Actualizar el vector g con los nuevos valores de theta y phig = [((W-B)*(sin(theta)));
((-(W-B))*cos(theta)*sin(phi));
((-(W-B))*cos(theta)*cos(phi));
(((-(yg*W)-(yb*B))*cos(theta)*cos(phi))+(((zg*W)-(zb*B))*cos(theta)*sin(phi)));
(((zg*W)-(zb*B))*sin(theta)+(((xg*W)-(xb*B))*cos(theta)*cos(phi)));
(((-(xg*W)-(xb*B))*cos(theta)*sin(phi))-(((yg*W)-(yb*B))*sin(theta)))];
g_deg = g * (180/pi);
v_pred = v(:, i) + h * acc(v(:, i));
v(:, i+1) = v(:, i) + h/2 * (acc(v(:, i)) + acc(v_pred));
p(:, i+1) = p(:, i) + h * v(:, i+1);
%a(:, i+1) = acc(v(:, i+1));end
figure(1)
%subplot(3, 1, 1)plot(t, p(1, :), 'r', t, p(2, :), 'g', t, p(3, :), 'b', t, p(4, :), 'm', t, p(5, :), 'c', t, p(6, :), 'k')
title('Posición')
xlabel('Tiempo (s)')
ylabel('Posición (m)')
legend('x', 'y', 'z', 'roll(x)', 'pitch(y)', 'yaw(z)')
figure(2)
%subplot(3, 1, 2)plot(t, v(1, :), 'r', t, v(2, :), 'g', t, v(3, :), 'b', t, v(4, :), 'm', t, v(5, :), 'c', t, v(6, :), 'k')
title('Velocidad')
xlabel('Tiempo (s)')
ylabel('Velocidad (m/s)')
legend('x', 'y', 'z', 'roll(x)', 'pitch(y)', 'yaw(z)')
/>Valora esta pregunta
0