/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_10.py
410 строк
15 KB
Chronos
Живо то, что ещё не умерло
28 мар 2025, 18:54
28 мар 2025, 18:54
3da24f4
Код
Авторство
О чём код?
from scipy.integrate import odeint import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from A_B_computation import solve_system from params import * from full_unimod_lib import * from scipy.linalg import expm # Матрицы системы 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] ]).T B = np.asarray([ [1], [1], [0], [0] ]) # Строим управление на основе дискретизованной системы h = 0.1 # Шаг дискретизации 0.1 - ок Ad = expm(A*h) print("Ad: \n", a2l.to_ltx(Ad,frmt='{:6.7f}')) # Bd = inv(A)@(Ad-np.eye(4))@B Bd = B print("Bd: \n", a2l.to_ltx(Bd,frmt='{:6.7f}')) # Итак, мы получили дискретную систему # Собственные числа желаемого спектра lambda -> exp(lambda*h) # Матрица дискретного(!) наблюдателя # C = np.asarray([[1.0,1.0,0.0,0.0]]) # Этап 1: строим управление для регулятора (теперь желаемый спектр дискретен) print("Управление для регулятора: \n") reg_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) # Желаемый спектр регулятора # reg_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) Q,Ac = full_unimodal_control_FL(Ad,Bd,reg_spec) # Управление для регулятора # Получили кусочно-постоянное управление на отрезке длины h Ac = Ac.T a1,a2,a3,a4 = Q[0] # theta1_ddot_, theta2_ddot_ = solve_system() # # def model(y, t): # theta1, theta2,theta1_dot, theta2_dot = y # 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 Ac@y # Начальные условия (близкие к нулю) # y0 = [1e-5,-1e-5, -1e-5, 0,0,0,0,0] # y0 = [0.001, 0.001, 0.001, 0.001] y0 = [1e-2, 1e-2, 1e-2, 1e-2] # Решение нелинейной системы # При дискретизации отдельно на каждом отрезке, полагая управление постоянным параметром от НУ time = 0.0 T = 20.0 init_cond = y0 # sol_nonlinear = np.asarray(init_cond) # times_nonlinear = np.asarray([0.0]) # V_values = [] # while time <= T: # theta1 = init_cond[0] # theta2 = init_cond[1] # theta1_dot = init_cond[2] # theta2_dot = init_cond[3] # V = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot # sol_nonlinear_part = odeint(model, init_cond, np.linspace(time, time+h,100)) # time += h # init_cond = sol_nonlinear_part[-1] # # print(sol_nonlinear_part.shape) # # print(sol_nonlinear.shape) # sol_nonlinear = np.vstack((sol_nonlinear,sol_nonlinear_part)) # # print(sol_nonlinear.shape) # V_values.append(V*np.ones(100)) # times_nonlinear = np.hstack((times_nonlinear,np.linspace(time, time+h,100))) # times_nonlinear =np.hstack((times_nonlinear,time)) # # V_values.append(V*np.ones(100)) # sol_nonlinear = np.asarray(sol_nonlinear) # V_values = np.asarray(V_values).flatten() # times_nonlinear = np.asarray(times_nonlinear).flatten()[:-1] # plt.figure(figsize=(14, 6)) # plt.subplot(1, 1, 1) # plt.xlabel('Время, с',fontsize = 30,fontfamily = "Times New Roman") # plt.ylabel('Напряжение V',fontsize = 30,fontfamily = "Times New Roman") # plt.xticks(fontfamily = "Times New Roman",fontsize = 20) # plt.yticks(fontfamily = "Times New Roman",fontsize = 20) # plt.plot(times_nonlinear, V_values, 'b') # plt.show() # Временной интервал # t = np.linspace(0, 10.0, 10_000) # Решение линеаризованной системы # sol_linear = odeint(linear_model, y0, t) time = 0.0 y = y0 sol_linear = np.asarray(y) while time <= T: y = Ac@y sol_linear = np.vstack((sol_linear,y)) time += h # Построение графиков # plt.figure(figsize=(14, 6)) # fs = 20 # # Фазовые портреты нелинейной системы # plt.subplot(1, 2, 1) # plt.plot(sol_nonlinear[:,0], sol_nonlinear[:,2], 'b') # # plt.plot(sol_nonlinear[:,4], sol_nonlinear[:,6], 'g',label="Вид наблюдателя") # plt.legend() # 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_nonlinear[:,1], sol_nonlinear[:,3], 'r') # # plt.plot(sol_nonlinear[:,5], sol_nonlinear[:,7], 'g',label="Вид наблюдателя") # plt.legend() # 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-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # plt.grid() # # plt.figure(figsize=(14, 6)) # # Фазовые портреты линеаризованной системы # plt.subplot(1, 2, 1) # plt.plot(sol_linear[:,0], sol_linear[:,2], 'b') # # plt.plot(sol_linear[:,4], sol_linear[:,6], 'g',label="Вид наблюдателя") # 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.legend() # plt.grid() # # plt.subplot(1, 2, 2) # plt.plot(sol_linear[:,1], sol_linear[:,3], 'r') # # plt.plot(sol_linear[:,5], sol_linear[:,7], 'g',label="Вид наблюдателя") # 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.legend() # plt.grid() # V_values = np.asarray(V_values).flatten() # times_nonlinear = np.asarray(times_nonlinear).flatten()[:-1] # # t = times_nonlinear # # 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], '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_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], '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() t = np.linspace(.0,T,sol_linear[:,0].shape[0]) 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], 'b') ax1.set_ylabel("$\\epsilon_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("$\\epsilon_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], 'b') ax3.set_ylabel("$\\epsilon_3$",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("$\\epsilon_4$",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() # # V_values = a1*sol_linear[:,0]+a2*sol_linear[:,1]+a3*sol_linear[:,2]+a4*sol_linear[:,3] # # # f,ax = plt.subplots(1,1) # f.suptitle("Управление",fontsize=20,fontfamily = "Times New Roman") # ax.plot(np.linspace(.0,T,V_values.shape[0]),V_values,'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 # Шаг прореживания данных 50 # t_anim = times_nonlinear[::step] # sol_anim = sol_nonlinear[::step] # print(sol_anim.shape) # # Создаем фигуру для анимации # fig_anim, ax = plt.subplots(figsize=(6,6)) # ax.set_xlim(-0.6, 0.6) # ax.set_ylim(0.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 # # line1_obs, = ax.plot([], [], 'g-', lw=2,alpha=0.2) # Первое звено # line2_obs, = ax.plot([], [], 'g-', lw=2,alpha=0.2) # Второе звено # point1_obs, = ax.plot([], [], 'go', markersize=10,alpha=0.2) # Шарнир 1 # point2_obs, = ax.plot([], [], 'go', markersize=10,alpha=0.2) # Шарнир 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([], []) # # line1_obs.set_data([], []) # line2_obs.set_data([], []) # point1_obs.set_data([], []) # point2_obs.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, 1] # # # Координаты первого звена # 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,line1_obs, line2_obs, point1_obs, point2_obs ,time_text # # # Создаем анимацию # ani = FuncAnimation(fig_anim, animate, frames=len(sol_anim)-1, # 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_9_b.mp4', fps=30) plt.tight_layout() plt.show()