/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_7.py
418 строк
15 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 import matplotlib from matplotlib.animation import FuncAnimation from params import * from full_unimod_lib import * from A_B_computation import * import control as ctrl # Матрицы системы # 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] ]) # 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") print(left_eigv[0]) print(left_eigv[1]) reg_spec = np.asarray([-1,-2, left_eigv[0], left_eigv[1]]) # Желаемый спектр регулятора Q,Ac = full_unimodal_control_FL(A,B,reg_spec) # Управление для регулятора print(np.linalg.eigvals(Ac)) # Этап 2: строим управление для наблюдателя print("Управление для наблюдателя: \n") obs_spec = np.asarray([-1,-1, left_eigv[0], left_eigv[1]]) # Желаемый спектр наблюдателя # [-10,-10, -21.421, -265.086] # L = ctrl.place(A.T,C.T,obs_spec) L,Ao = full_unimodal_control_FL(A.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): theta1, theta2,theta1dot, theta2dot,zeta1, zeta2, zeta3, zeta4 = 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) # zeta_arr_dot = A@zeta_arr+B.T*V+(L@C)@(theta_arr-zeta_arr) # print(np.linalg.eigvals(A+B@Q-L@C)) # print("Спектр") # Общий знаменатель # 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 # theta1_ddot = theta1_ddot_.subs({theta1_dot:theta1dot,theta2_dot:theta2dot}) # theta2_ddot = theta2_ddot_.subs({theta1_dot:theta1dot,theta2_dot:theta2dot}) return [theta1dot, theta2dot,theta1_ddot_(V,theta1, theta2,theta1dot,theta2dot), theta2_ddot_(V,theta1, theta2,theta1dot,theta2dot), dzetas[0][0],dzetas[1][0],dzetas[2][0],dzetas[3][0]] 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, 0.0 , 0.0 ,0.0 ,0.0],dtype = np.float64) # Решение нелинейной системы # print("Вектор в начале:") # print(model(y0,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]] print(t_stab) normed = np.max(np.abs(np.asarray(sol_nonlinear).flatten())) print(normed) # Решение линеаризованной системы sol_linear = odeint(linear_model, y0, t) # Построение графиков 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, sol_nonlinear[:,4], '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, sol_nonlinear[:,5], '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, sol_nonlinear[:,6], '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, sol_nonlinear[:,7], '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, sol_linear[:,0], 'r') # ax1.plot(t, sol_linear[:,4], '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_linear[:,1], 'r') # ax2.plot(t, sol_linear[:,5], '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_linear[:,2], 'r') # ax3.plot(t, sol_linear[:,6], '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_linear[:,3], 'r') # ax4.plot(t, sol_linear[:,7], '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,a1*sol_nonlinear[:,4] + a2*sol_nonlinear[:,5] + a3*sol_nonlinear[:,6] + a4*sol_nonlinear[:,7],'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 = sol_anim[i, 4] # zeta2 = sol_anim[i, 5] # # # Координаты первого звена # 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 # # # Создаем анимацию # 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_7.mp4', fps=30) plt.tight_layout() plt.show()