task_4.py - master - Chronos/TU_laba

task_4.py
129 строк4 KB
Chronos
Живо то, что ещё не умерло
28 мар 2025, 09:12
28 мар 2025, 09:12f49a0f7
# task 4: строим линеаризованную систему

# ЭТАП 1: решаем символьно систему

from sympy import symbols, sin, cos, Matrix, simplify, latex, Eq, solve,Wild,print_latex
from sympy.physics.vector import dynamicsymbols
import numpy as np

import params as par

# Настраиваем вывод LaTeX для производных
theta1, theta2 = dynamicsymbols('theta1 theta2')  # Автоматически добавляет точки для производных
theta1_dot = symbols('theta1_dot', latex_name=r'\dot{\theta}_1')
theta2_dot = symbols('theta2_dot', latex_name=r'\dot{\theta}_2')

# Объявляем параметры системы
A11, A12, A22 = symbols('A11 A12 A22')
g, m1, m2, l1, L1, l2, B1, B2, Kf, Ks, V = symbols(
    'g m1 m2 l1 L1 l2 B1 B2 K_f K_s V',
    latex_names=[r'g', r'm_1', r'm_2', r'l_1', r'L_1', r'l_2',
                 r'B_1', r'B_2', r'K_f', r'K_s', r'V']
)


x, y = symbols('x y')  # Неизвестные переменные

# Правые части уравнений
C1 = g*(m1*l1 + m2*L1)*sin(theta1) + Kf*(V-Ks*theta1_dot) - B1*theta1_dot-A12*theta2_dot*theta2_dot*sin(theta1-theta2)
C2 = m2*g*l2*sin(theta2) - B2*theta2_dot+A12*theta1_dot*theta1_dot*sin(theta1-theta2)

print("A11",par.A11)
print("A12",par.A12)
print("A22",par.A22)

# Создаем систему уравнений
system = [
    Eq(A11*x + A12*cos(theta1 - theta2)*y, C1),
    Eq(A12*cos(theta1 - theta2)*x + A22*y, C2)
]

# Решаем систему символьно
solution = solve(system, [x, y])

# Выводим результат
print("Решение системы:")
print_latex(solution[x].simplify())
print_latex(solution[y].simplify())


# ЭТАП 2: вычисляем символьно якобиан системы
# theta1_ddot = solution[x].simplify()
# theta2_ddot = solution[y].simplify()
theta1_ddot = solution[x]
theta2_ddot = solution[y]

# Формируем вектор состояния: [theta1, theta2, theta1_dot, theta2_dot]
state = [theta1, theta2, theta1_dot, theta2_dot]
f = Matrix([theta1_dot, theta2_dot, theta1_ddot, theta2_ddot])

# Вычисляем якобиан
J = f.jacobian(state)
print("Символьный якобиан системы:")
print(latex(J))  # Для Jupyter. Для обычного Python: print(latex(J))

# Точность вывода
k = 7 # Количество знаков после запятой
rf = lambda per: per.round(k)

# Переберём все состояния равновесия
print("\n")
for theta1_val in [0, np.pi]:
    for theta2_val in [0, np.pi]:
        print("Для состояния равновесия (theta1={}, theta2={}):".format(theta1_val, theta2_val))
        print("Символьный якобиан системы:")
        J_symbolic = J.subs({
            theta1_dot: 0,
            theta2_dot: 0,
            theta1: theta1_val,
            theta2: theta2_val,
            V: 0
        })
        print(latex(J_symbolic))
        print("Численный якобиан системы:")
        J_numeric = J.subs({
            theta1_dot: 0,
            theta2_dot: 0,
            Kf: par.Kf,
            Ks: par.Ks,
            m1: par.m1,
            m2: par.m2,
            g: par.g,
            l1: par.l1,
            l2: par.l2,
            L1: par.L1,
            A11: par.A11,
            A12: par.A12,
            A22: par.A22,
            B1: par.B1,
            B2: par.B2,
            theta1: theta1_val,
            theta2: theta2_val,
            V:0
        })
        print(latex(J_numeric.applyfunc(rf)))
        print("Собственные числа якобиана системы:")
        eigenvalues = np.asarray(list(J_numeric.eigenvals()))
        # print(list(eigenvalues))
        rf_vec = np.vectorize(rf)
        print(rf_vec(eigenvalues))


        print("\n")
print("Символьная матрица B для СР [0,0]:")
B_symbolic = Matrix([
    0,
    0,
    A22*Kf/(A11*A22-A12**2),
    -A12*Kf/(A11*A22-A12**2)
])
print(latex(B_symbolic))
print("\n")
print("Численная матрица B для СР [0,0]:")
B_numeric = B_symbolic.subs({
    Kf: par.Kf,
    A11: par.A11,
    A12: par.A12,
    A22: par.A22
})
print(latex(B_numeric.applyfunc(rf)))