/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_5.py
278 строк
10 KB
Chronos
Нет лучше волка, чем собака друга
16 апр 2025, 22:02
16 апр 2025, 22:02
0d7ddc7
Код
Авторство
О чём код?
import numpy as np from scipy.integrate import odeint import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from full_unimod_lib import * from params import * from A_B_computation import * # Параметры системы (задайте свои значения) # Точность вывода k = 7 # Количество знаков после запятой # Линеаризованная система имеет матрицы A = np.asarray([ [0 , 0 , 1 , 0], [0 , 0 , 0 , 1], [74.4982357 , -38.2565903, -56.0726559, 86.3428671], [-114.7697709,101.3459259,86.3838696, -228.7317749] ]) B = np.asarray([ [0], [0], [0.9505409], [-1.464375] ]) theta1_ddot_, theta2_ddot_ = solve_system() # Применим метод через форму Фробениуса-Луенбергера spec = np.asarray([-1,0,-21.421,-265.086]) # Желаемый спектр: ЗДЕСЬ МЕНЯТЬ КОРНИ! Q,Ac = full_unimodal_control_FL(A,B,spec) # print(np.linalg.eigvals(Ac)) a1, a2, a3, a4 = Q[0] # Коэффициенты управления def model(y, t): theta1, theta2,theta1_dot, theta2_dot = y # Вычисление управляющего напряжения V V = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot # # Общий знаменатель # delta_theta = theta1 - theta2 # denominator = A11 * A22 - (A12**2) * np.cos(delta_theta)**2 # # # Расчет theta1_ddot # numerator1 = A12 * B2 * theta2_dot * np.cos(delta_theta)- A12 * g * l2 * m2 * np.sin(theta2) * np.cos(delta_theta)- A22 * (B1 + Kf*Ks) * theta1_dot+ A22 * g * (L1 * m2 + l1 * m1) * np.sin(theta1)+ A22 * Kf * V # # theta1_ddot = numerator1 / denominator # # # Расчет theta2_ddot # numerator2 = -A11 * B2 * theta2_dot+ A11 * g * l2 * m2 * np.sin(theta2)+ A12 * theta1_dot * (B1 + Kf*Ks) * np.cos(delta_theta)- A12 * g * (m2*L1 + m1*l1) * np.sin(theta1) * np.cos(delta_theta)- A12 * Kf * V * np.cos(delta_theta) # # theta2_ddot = numerator2 / denominator return [theta1_dot, theta2_dot,theta1_ddot_(V,theta1, theta2,theta1_dot,theta2_dot), theta2_ddot_(V,theta1, theta2,theta1_dot,theta2_dot)] def linear_model(y, t): return np.dot(Ac, y) # Временной интервал t = np.linspace(0, 10, 10_000) # Начальные условия (близкие к нулю) y0 = [0.001,0.0001, 0, 0] # y0 = [[0.1,0.1, 0, 0] # Решение нелинейной системы sol_nonlinear = odeint(model, y0, t) print(sol_nonlinear) # Время стабилизации t_stab = np.linalg.norm(sol_nonlinear, axis=1) epsilon = 1e-3 # print(np.where(t_stab < epsilon)) # t_stab = t[np.where(t_stab < epsilon)[0][0]] # print(t_stab) normed = np.max(np.abs(np.asarray(sol_nonlinear).flatten())) print(normed) # Решение линеаризованной системы sol_linear = odeint(linear_model, y0, t) # Построение графиков # plt.figure() f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) # f.suptitle('Нелинейная система', fontsize=20,fontfamily = "Times New Roman") fs = 20 # Фазовые портреты нелинейной системы # plt.subplot(2, 2, 1) ax1.plot(t, sol_nonlinear[:,0], 'b') ax1.set_ylabel("$\\theta_1$",fontsize = fs) ax1.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax1.grid() # plt.subplot(2, 2, 2) ax2.plot(t, sol_nonlinear[:,1], 'b') ax2.set_ylabel("$\\theta_2$",fontsize = fs) ax2.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax2.grid() # plt.subplot(2, 2, 3) ax3.plot(t, sol_nonlinear[:,2], 'b') ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) ax3.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax3.grid() # plt.subplot(2, 2, 4) ax4.plot(t, sol_nonlinear[:,3], 'b') ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) ax4.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax4.grid() # f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) # f.suptitle('Линейная система', fontsize=20,fontfamily = "Times New Roman") fs = 20 # Фазовые портреты нелинейной системы # plt.subplot(2, 2, 1) ax1.plot(t, sol_linear[:,0], 'r') ax1.set_ylabel("$\\theta_1$",fontsize = fs) ax1.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax1.grid() # plt.subplot(2, 2, 2) ax2.plot(t, sol_linear[:,1], 'r') ax2.set_ylabel("$\\theta_2$",fontsize = fs) ax2.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax2.grid() # plt.subplot(2, 2, 3) ax3.plot(t, sol_linear[:,2], 'r') ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) ax3.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax3.grid() # plt.subplot(2, 2, 4) ax4.plot(t, sol_linear[:,3], 'r') ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) ax4.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax4.grid() f,ax = plt.subplots(1,1) f.suptitle("Управление",fontsize=20,fontfamily = "Times New Roman") ax.plot(t,a1*sol_nonlinear[:,0] + a2*sol_nonlinear[:,1] + a3*sol_nonlinear[:,2] + a4*sol_nonlinear[:,3],'b') ax.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax.set_xlabel('$t$',fontsize = fs) ax.set_ylabel('$V$',fontsize = fs) ax.grid() # step = 10 # Шаг прореживания данных # t_anim = t[::step] # sol_anim = sol_nonlinear[::step] # # # Создаем фигуру для анимации # fig_anim, ax = plt.subplots(figsize=(6,6)) # ax.set_xlim(-0.6, 0.6) # ax.set_ylim(0, L1+L2+0.5) # ax.set_aspect('equal') # ax.grid() # # # Создаем элементы анимации # line1, = ax.plot([], [], 'b-', lw=2) # Первое звено # line2, = ax.plot([], [], 'r-', lw=2) # Второе звено # point1, = ax.plot([], [], 'bo', markersize=10) # Шарнир 1 # point2, = ax.plot([], [], 'ro', markersize=10) # Шарнир 2 # time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes) # # def init(): # line1.set_data([], []) # line2.set_data([], []) # point1.set_data([], []) # point2.set_data([], []) # time_text.set_text('') # return line1, line2, point1, point2, time_text # # def animate(i): # theta1 = sol_anim[i, 0] # theta2 = sol_anim[i, 2] # # # Координаты первого звена # x1 = L1 * np.sin(theta1) # y1 = L1 * np.cos(theta1) # # # Координаты второго звена # x2 = x1 + L2 * np.sin(theta2) # y2 = y1 + L2 * np.cos(theta2) # # # Обновление линий # line1.set_data([0, x1], [0, y1]) # line2.set_data([x1, x2], [y1, y2]) # # # Обновление точек # point1.set_data([x1], [y1]) # point2.set_data([x2], [y2]) # # time_text.set_text(f'Время = {t_anim[i]:.2f} с') # return line1, line2, point1, point2, time_text # # # Создаем анимацию # ani = FuncAnimation(fig_anim, animate, frames=len(sol_anim), # init_func=init, blit=True, interval=20) # ДЛЯ СОХРАНЕНИЯ АНИМАЦИИ !!!!! # plt.rcParams['animation.ffmpeg_path'] = 'C:\\ffmpeg\\ffmpeg-master-latest-win64-gpl-shared\\bin\\ffmpeg.exe' # ani.save('task_5d.mp4', fps=30) plt.tight_layout() plt.show()