Sumulacja Rakiety w programie SimStructure

pliki:

simstrucuture.zip

ardugeek_rocket1.zip

# Opis działania kodu sterującego

Ten dokument opisuje fragment kodu sterującego dla pojazdu (np. drona lub rakiety) z wykorzystaniem regulatorów PID w dwóch osiach: pionowej (Y) oraz kąta nachylenia (Angle).

## Parametry PID dla osi Y

Kod definiuje następujące zmienne sterujące ruchem w osi pionowej:

- desired_y – zadana pozycja w osi Y. Początkowo ustawiona na 10, następnie dynamicznie zmieniana:

<code>desired_y = 15 + 5 * t;</code>

- error_y – błąd położenia:

<code>error_y = desired_y - p1.y;</code>

- Współczynniki regulatora PID:

*Proporcjonalny: <code>kp_y = 1</code> Różniczkujący: <code>kd_y = 1</code> Całkujący: <code>ki_y = 0.3</code> - i_y – wartość całki błędu (integrator) ## Sterowanie siłą ciągu (thrust) Siła ciągu obliczana jest jako: <code>t1.thrust = t1.saturation * ( kp_y * error_y – kd_y * p1.dy + ki_y * i_y ); thrust = t1.thrust / 100000;</code> - <code>t1.saturation</code> to limit sygnału wyjściowego silnika. - Wynik dzielony jest przez 100 000, aby uzyskać wartość w [N]. ## Regulacja kąta nachylenia Sterowanie kątem realizowane jest osobnym regulatorem PID: - a – żądany kąt: <code>[]a = pi - (pi/180) * b1.heading + 10 * (key2(„1”) - key2(„2”));</code> - Współczynniki PID: Proporcjonalny: <code>kp_a = 1.0</code> Różniczkujący: <code>kd_a = 0.01</code> Całkujący: <code>ki_a = 0.001</code> - angle_err – przeskalowany błąd kąta: <code>angle_err = 50 * a;</code> - i_a – integrator kąta: <code>i_a = a / 10;</code> - Obliczenie sygnału sterującego kątem: <code>t1.angle = kp_a * a - kd_a * b1.spin + ki_a * i_a; t1_angle = 50 * t1.angle;</code> ## Część wizualizacyjna W bloku graficznym rysowane są: - Pozioma linia w pozycji desired_y**:

line(p1.x-1000, desired_y,
p1.x+1000, desired_y,
blue, #3)

- Dynamiczne teksty:

<code>text(#10,#50, "Thrust: %12.6f N", t1.thrust)

text(#10,#10, „Heading: %12.6f°”, b1.heading) text(#10,#30, „Angle: %12.6f rad”, a) text(#10,#70, „Integrator: %12.6f”, i_y)</code> - Wykresy sygnałów:

<code>plot s_y, thrust, angle_err, t1_angle</code>

## Podsumowanie

- Sterowanie położeniem w osi Y realizowane jest przez regulator PID (P, I, D). - Sterowanie kątem nachylenia realizowane jest przez osobny regulator PID. - Blok wizualizacyjny umożliwia monitorowanie kluczowych parametrów w czasie rzeczywistym.

Kod:

double desired_y=10,error_y,kd_y=1,kp_y=1,ki_y=0.3; signal s_y; signal thrust; signal t1_angle; integrator i_y; desired_y = 15+5*t; error_y = desired_y-p1.y; s_y = error_y; i_y = error_y; t1.thrust = t1.saturation*(kp_y*error_y-kd_y*p1.dy+ki_y*i_y); thrust = t1.thrust/100000; double a,kp_a=1.0,kd_a=0.01,ki_a=0.001,a_err; []a = pi-pi/180*b1.heading+10*(key2("1")-key2("2")); signal angle_err; angle_err = 50*a; integrator i_a; i_a = a/10; t1.angle = kp_a*a-kd_a*b1.spin+ki_a*i_a; t1_angle = 50*t1.angle; @{ line(p1.x-1000,desired_y,p1.x+1000,desired_y,blue,#3); text(#10,#50,"Thrust: %12.6f N",t1.thrust); text(#10,#10,"Heading:%12.6f degree",b1.heading); text(#10,#30,"Angle: %12.6f rad",a); text(#10,#70,"Integrator: %12.6f",i_y); plot s_y, thrust, angle_err, t1_angle; }