/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_11.py
476 строк
16 KB
Chronos
Нет лучше волка, чем собака друга
16 апр 2025, 22:02
16 апр 2025, 22:02
0d7ddc7
Код
Авторство
О чём код?
from scipy.integrate import odeint import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from params import * from full_unimod_lib import * from A_B_computation import * from scipy.linalg import expm # Матрицы системы # A = np.asarray([ # [0, 0, 1, 0], # [0, 0, 0, 1], # [74.574, -38.295, -56.072, 86.342], # [-114.886, 101.449, 86.383, -228.731] # ]) # Численный якобиан системы: # \left[\begin{matrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\74.5742544 & -38.2956276 & -1120.9474312 & 1726.857341\\-114.8868829 & 101.4493401 & 1726.8983435 & -4574.6354981\end{matrix}\right] # A = np.asarray([ # [0, 0, 1, 0], # [0, 0, 0, 1], # [74.5742544 , -38.2956276 , -1120.9474312 , 1726.857341], # [-114.8868829 , 101.4493401 ,1726.8983435 , -4574.6354981] # ]) # B = np.asarray([ # [0], # [0], # [0.9505409], # [-1.464375] # ]) 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] ]) h = 0.1 Ad = expm(A*h) # print("Ad: \n", a2l.to_ltx(Ad,frmt='{:6.7f}')) Bd = inv(A)@(Ad-np.eye(4))@B # A,B = compute_A_B() np.set_printoptions(precision=10,suppress=True) print(A) print(B) print("Исходный спектр матрицы A: \n") eigv = np.linalg.eigvals(A) print(eigv) left_eigv = eigv[eigv < 0.0] # print(eigv[eigv < 0.0]) # Матрица наблюдателя C = np.asarray([[1.0,1.0,0.0,0.0]]) # C = np.asarray([ # [1.0,0.0,0.0,0.0], # [0.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) # Управление для регулятора # Q,Ac = full_unimodal_control_FL(A,B,reg_spec) # Управление для регулятора # print(np.linalg.eigvals(Ac)) # Этап 2: строим управление для наблюдателя print("Управление для наблюдателя: \n") obs_spec = np.asarray([0.9,0.9, np.exp(-21.421*h), np.exp(-265.086*h)]) # Желаемый спектр наблюдателя # [-10,-10, -21.421, -265.086] # L = ctrl.place(A.T,C.T,obs_spec) L,Ao = full_unimodal_control_FL(Ad.T,C.T,obs_spec) # Управление для наблюдателя # print(np.linalg.eigvals(Ao)) L = -L.T # L = L*0 # L = L.reshape(4,1) # Этап 3: строим всю систему целиком a1,a2,a3,a4 = Q[0] theta1_ddot_, theta2_ddot_ = solve_system() def model(y, t,V): theta1, theta2,theta1dot, theta2dot = y # Вычисление управляющего напряжения V (по состоянию наблюдателя) # V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # V = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot # Как меняется состояние наблюдателя # zeta_arr = np.array([[zeta1], [zeta2], [zeta3], [zeta4]]) # theta_arr = np.array([[theta1], [theta2], [theta1dot], [theta2dot]]) # # dzetas = A@zeta_arr+B*V+(L@C)@(theta_arr-zeta_arr) return [theta1dot, theta2dot,theta1_ddot_(V,theta1, theta2,theta1dot,theta2dot), theta2_ddot_(V,theta1, theta2,theta1dot,theta2dot)] # def linear_model(y, t): # theta_arr = y[:4].reshape(4,1) # # print(theta_arr) # zeta_arr = y[4:].reshape(4,1) # # print(zeta_arr) # V = (Q@zeta_arr) # # print("V",V) # theta_arr_dot = A@theta_arr+B*V # # print("theta_arr_dot",theta_arr_dot) # zeta_arr_dot = A@zeta_arr+B*V+(L@C)@(theta_arr-zeta_arr) # # print("zeta_arr_dot",zeta_arr_dot) # res = np.vstack((theta_arr_dot,zeta_arr_dot)).flatten() # # print(res) # return res # Q = Q.reshape(1,4) # print("Матрица всей линейной системы по принципу разделения:") # M_top = np.hstack((A,B@Q)) # M_bot = np.hstack((L@C,A+B@Q-L@C)) # M = np.vstack((M_top,M_bot)) # print(M) # print("Спектр всей системы по принципу разделения:") # print(np.linalg.eigvals(M)) # Временной интервал # t = np.linspace(0, 20.0, 20_000) # Начальные условия (близкие к нулю) # y0 = [1e-5,-1e-5, -1e-5, 0,0,0,0,0] # y0 = [0.001, 0.001, 0.001, 0.001] y0 = np.asarray([0.00032, 0.0, 0.0, 0.0]) # Решение нелинейной системы # print("Вектор в начале:") # print(model(y0,0)) time = 0.0 zetas = np.asarray([ [0], [0], [0], [0] ]) sol_nonlinear = np.asarray([y0]) zeta_arr = zetas.T while time <= 20: zeta1,zeta2,zeta3,zeta4 = zetas.reshape(4) V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # print(Q[0]) # print(zetas) # print(V) # print(model(y0,time,V)) nonlin_sol = odeint(model, y0, np.linspace(time,time+h,100), args=(V,)) sol_nonlinear = np.vstack((sol_nonlinear,nonlin_sol)) # print(np.tile(zetas.T,(100,1))) zeta_arr = np.vstack((zeta_arr,np.tile(zetas.T,(100,1)))) # print(Ad) # print(y0.reshape(4,1)-zetas) # print(Ad@zetas) zetas = Ad@zetas+Bd*V+(L@C)@(y0.reshape(4,1)-zetas) y0 = nonlin_sol[-1] time += h # zeta_arr = zeta_arr[1:,:] t = np.linspace(0, 20.0, len(sol_nonlinear)) # sol_nonlinear = odeint(model, y0, t) # Время стабилизации print() t_stab = np.linalg.norm(sol_nonlinear, axis=1) epsilon = 0.0013 # print(np.where(t_stab < epsilon)) t_stab = t[np.where(t_stab < epsilon)] print(t_stab) normed = np.max(np.abs(np.asarray(sol_nonlinear).flatten())) print(normed) # Решение линеаризованной системы sol_linear = np.asarray([y0]) time = 0.0 zetas = np.asarray([ [0], [0], [0], [0] ]) # sol_nonlinear = np.asarray([y0]) thetas = y0.reshape(4,1) linear_zeta_arr = zetas.T V_values = np.asarray([0]) while time <= 20: zeta1,zeta2,zeta3,zeta4 = zetas.reshape(4) # print("zetas:",zetas) V = a1*zeta1 + a2*zeta2 + a3*zeta3 + a4*zeta4 # nonlin_sol = odeint(model, y0, np.linspace(time,time+h,100), args=V) V_values = np.hstack((V_values,np.tile(V,(100)))) zetas = Ad@zetas+Bd*V+(L@C)@(thetas-zetas) linear_zeta_arr = np.vstack((linear_zeta_arr,zetas.T)) # print(thetas) # print(Ad) # print(Ad@thetas) # print(V) thetas = Ad@thetas+Bd*V sol_linear = np.vstack((sol_linear,thetas.T)) # y0 = nonlin_sol[-1] time += h # print(sol_linear.shape) t_lin = np.linspace(0, 20.0, len(sol_linear)) # t_lin_obs = np.linspace(0, 20.0, len(linear_zeta_arr)) # Построение графиков 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',label="Реальное состояние") ax1.plot(t, zeta_arr[:,0], 'g',label="Вид наблюдателя") 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() ax1.legend() # plt.subplot(2, 2, 2) ax2.plot(t, sol_nonlinear[:,1], 'b',label="Реальное состояние") ax2.plot(t, zeta_arr[:,1], 'g',label="Вид наблюдателя") 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() ax2.legend() # plt.subplot(2, 2, 3) # ax3.plot(t, sol_nonlinear[:,2], 'b') ax3.plot(t, sol_nonlinear[:,2], 'b',label="Реальное состояние") ax3.plot(t, zeta_arr[:,2], 'g',label="Вид наблюдателя") 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() ax3.legend() # plt.subplot(2, 2, 4) # ax4.plot(t, sol_nonlinear[:,3], 'r') ax4.plot(t, sol_nonlinear[:,3], 'b',label="Реальное состояние") ax4.plot(t, zeta_arr[:,3], 'g',label="Вид наблюдателя") 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() ax4.legend() # 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_lin, sol_linear[:,0], 'r') # ax1.plot(t_lin, linear_zeta_arr[:,0], 'g',label="Вид наблюдателя") 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() ax1.legend() # plt.subplot(2, 2, 2) ax2.plot(t_lin, sol_linear[:,1], 'r') # ax2.plot(t_lin, linear_zeta_arr[:,1], 'g',label="Вид наблюдателя") 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() ax2.legend() # plt.subplot(2, 2, 3) # ax3.plot(t, sol_nonlinear[:,2], 'b') ax3.plot(t_lin, sol_linear[:,2], 'r') # ax3.plot(t_lin, linear_zeta_arr[:,2], 'g',label="Вид наблюдателя") 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() ax3.legend() # plt.subplot(2, 2, 4) # ax4.plot(t, sol_nonlinear[:,3], 'r') ax4.plot(t_lin, sol_linear[:,3], 'r') # ax4.plot(t_lin, linear_zeta_arr[:,3], 'g',label="Вид наблюдателя") 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() ax4.legend() f,ax = plt.subplots(1,1) f.suptitle("Управление",fontsize=20,fontfamily = "Times New Roman") ax.plot(t,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 # Шаг прореживания данных # 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.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] # # zeta1 = zeta_arr[i, 0] # zeta2 = zeta_arr[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]) # # # Координаты первого звена # x1_obs = L1 * np.sin(zeta1) # y1_obs = L1 * np.cos(zeta1) # # # Координаты второго звена # x2_obs = x1_obs + L2 * np.sin(zeta2) # y2_obs = y1_obs + L2 * np.cos(zeta2) # # # Обновление линий # line1_obs.set_data([0, x1_obs], [0, y1_obs]) # line2_obs.set_data([x1_obs, x2_obs], [y1_obs, y2_obs]) # # # Обновление точек # point1_obs.set_data([x1_obs], [y1_obs]) # point2_obs.set_data([x2_obs], [y2_obs]) # # time_text.set_text(f'Время = {t_anim[i]:.2f} с') # return line1, line2, point1, point2,line1_obs, line2_obs, point1_obs, point2_obs ,time_text # # 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_11.mp4', fps=30) plt.tight_layout() plt.show()