/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_13.py
318 строк
11 KB
Chronos
Нет лучше волка, чем собака друга
15 апр 2025, 16:18
15 апр 2025, 16:18
7094f27
Код
Авторство
О чём код?
import numpy as np from matplotlib.animation import FuncAnimation from scipy.integrate import odeint import matplotlib.pyplot as plt from scipy import linalg import array_to_latex as a2l from A_B_computation import solve_system from params import * rho = 100.0 # Коэффициент задачи 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] ]) Q = np.diag([1,1,1,1,1]) R = rho A = np.vstack((np.hstack((A,B)),np.zeros(5))) B = np.asarray([[0],[0],[0],[0],[1]]) print(A) X = linalg.solve_continuous_are(A,B,Q,R) # Решение матричного уравнения Риккати print("Решение матричного уравнения Риккати:") print(a2l.to_ltx(X, frmt = '{:6.7f}', arraytype = 'bmatrix')) print("Соответствующее значение матрицы линейной обратной связи:") theta = -1/R*B.T@X print(a2l.to_ltx(theta, frmt = '{:6.7f}', arraytype = 'bmatrix')) Ac = A-(1/R)*B@B.T@X print("Матрица замкнутой системы:") print(a2l.to_ltx(Ac, frmt = '{:6.7f}', arraytype = 'bmatrix')) # Матрица системы # Ac = np.asarray([[ 0. , 0. , 1. , 0. ], # [ 0. , 0. , 0. , 1. ], # [ 130.204, 64709.559, 729.585, 730.534], # [ -200.615, -99678.402, -1124.356, -1221.465]]) fs = 20 a1,a2,a3,a4,a5 = theta.flatten() theta1_ddot_, theta2_ddot_ = solve_system() def model(y, t): theta1, theta2,theta1_dot, theta2_dot,V = y # V_dot = # Вычисление управляющего напряжения V V_dot = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot+a5*V # # Общий знаменатель # 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), V_dot] def f(y, t): return np.dot(Ac, y) # Начальные условия y0 = np.asarray([0.01, 0.0006, 0, 0,0.0]) t = np.linspace(0, 60, 60_000) sol_linear = odeint(f, y0, t) sol_nonlinear = odeint(model, y0, t) np.savetxt('task_13_nonlinear_d.csv', sol_nonlinear, delimiter=' ') 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,sol_nonlinear[:,4],'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_13d.mp4', fps=30) # plt.plot(t, sol_linear[:,0], 'b', label='$\\theta_1$') # plt.plot(t, sol_linear[:,1], 'r', label='$\\theta_2$') # plt.plot(t, sol_linear[:,2], 'g', label='$u$') # plt.xlabel("$t$",fontsize = fs) # plt.legend(fontsize = fs) # # plt.figure(figsize=(14, 6)) # # # Фазовые портреты линеаризованной системы # # plt.subplot(1, 2, 1) # # plt.plot(sol_linear[:,0], sol_linear[:,1], 'g') # # plt.xlabel("$\\theta_1$",fontsize = fs) # # plt.ylabel('$\\dot{\\theta}_1$',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) # # plt.grid() # # # # plt.subplot(1, 2, 2) # # plt.plot(sol_linear[:,2], sol_linear[:,3], 'm') # # plt.xlabel("$\\theta_2$",fontsize = fs) # # plt.ylabel('$\\dot{\\theta}_2$',fontsize = fs) # # plt.title('Линеаризованная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # plt.grid() plt.tight_layout() plt.show()