/
Logrus
/
CopterControl
Обзор
Документация
Войти
/
Logrus
/
CopterControl
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
CI/CD
Аналитика
Безопасность
master
main.py
777 строк
27 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:17
99ef28c
Код
Авторство
О чём код?
import numpy as np import copy from scipy.interpolate import CubicSpline from scipy.optimize import minimize from jitfunc import * from constants import * pos_des_prev= np.zeros(3) # derivative filter state for vel prev_vel_err_z = 0.0 d_vel_err_z = 0.0 prev_vel_err_xy = np.zeros(2) d_vel_err_xy = np.zeros(2) phi_prev,theta_prev,psi_prev=np.zeros(3) T_sim = None#58 #t_points=[0,0.15*T_sim,0.35*T_sim,0.55*T_sim,0.76*T_sim,T_sim] waypoints = [ [0, 0, 0], [0, 0, 10], [10, 5, 15], [20, 0, 10], [10, -10, 5], [0, 0, 1] ] # waypoints = [ # [0, 0, 10], # [0, 0, 15], # # [0, 0, 10], # # [0, 0, 5], # # [0, 0, 10] # ] I = np.array([ [0.4 * m * rc * rc + 2 * l * l * m_p, 0, 0], [0, 0.4 * m * rc * rc + 2 * l * l * m_p, 0], [0, 0, 0.4 * m * rc * rc + 4 * l * l * m_p] ]) # Тензор инерции (кг*м^2) # Коэффициенты ПД-регулятора (нужно будет настроить) # Kp_pos = 0.3 # Kd_pos = 0.8 # Kp_ang = 0.4 # Kd_ang = 0.8 class TrajectoryPlanner: def __init__(self, waypoints, total_time, points_per_segment=20, t_points=None, auto_time_estimation=True): self.waypoints = np.array(waypoints) self.total_time = total_time if auto_time_estimation and t_points is None: # Автоматическая оценка времени t_points = self.auto_estimate_t_points() self.t_points = np.array(t_points) self.n_seg = len(waypoints) - 1 self.dim = 3 # создаём промежуточные точки между waypoints для сглаживания #self.full_waypoints = [] #self.t_points = [] #dt = total_time / ((len(waypoints)-1)*points_per_segment) # t = 0.0 # for i in range(len(waypoints)-1): # start = self.waypoints[i] # end = self.waypoints[i+1] # for j in range(points_per_segment): # alpha = j / points_per_segment # point = (1-alpha)*start + alpha*end # self.full_waypoints.append(point) # self.t_points.append(t) # t += dt # self.full_waypoints.append(self.waypoints[-1]) # self.t_points.append(total_time) self.full_waypoints = np.array(self.full_waypoints) self.t_points = np.array(self.t_points) # создаём кубические сплайны по XYZ self.cs_x = CubicSpline(self.t_points, self.full_waypoints[:, 0], bc_type='clamped') self.cs_y = CubicSpline(self.t_points, self.full_waypoints[:, 1], bc_type='clamped') self.cs_z = CubicSpline(self.t_points, self.full_waypoints[:, 2], bc_type='clamped') def evaluate(self, t): t = np.clip(t, 0, self.total_time) pos_des = np.array([self.cs_x(t), self.cs_y(t), self.cs_z(t)]) vel_des = np.array([self.cs_x(t, 1), self.cs_y(t, 1), self.cs_z(t, 1)]) acc_des = np.array([self.cs_x(t, 2), self.cs_y(t, 2), self.cs_z(t, 2)]) # yaw вдоль траектории с защитой от деления на ноль speed_xy = np.linalg.norm(vel_des[:2]) if speed_xy < 1e-3: psi_des = 0.0 else: psi_des = np.arctan2(vel_des[1], vel_des[0]) return pos_des, vel_des, acc_des, psi_des def auto_estimate_t_points(self, v_max=4.0, a_max=6.0, safety_margin=1.6): """Автоматическая оценка времени сегментов""" t_points = [0] cumulative_time = 0 for i in range(len(self.waypoints)-1): start = self.waypoints[i] end = self.waypoints[i+1] distance = np.linalg.norm(end - start) # Оценка времени с учетом динамических ограничений if distance > v_max**2 / a_max: t_segment = (distance / v_max + v_max / a_max) * safety_margin else: t_segment = (2 * np.sqrt(distance / a_max)) * safety_margin cumulative_time += t_segment t_points.append(cumulative_time) return t_points def get_trajectory_info(self): """Возвращает информацию о траектории""" total_distance = 0 for i in range(len(self.waypoints)-1): dist = np.linalg.norm(self.waypoints[i+1] - self.waypoints[i]) total_distance += dist avg_speed = total_distance / self.t_points[-1] if self.t_points[-1] > 0 else 0 return { 'total_time': self.t_points[-1], 'total_distance': total_distance, 'average_speed': avg_speed, 'segments': len(self.waypoints) - 1 } def check_trajectory_feasibility(planner, m=m, F_MAX=F_MAX, g=9.81, T_sim=None, N=200, max_tilt_deg=30): """ Проверяет, выполнима ли траектория при данных ограничениях по тяге и углам. planner : объект с методом evaluate(t) -> (pos, vel, acc, yaw) m : масса коптера (кг) F_MAX : максимальная суммарная тяга (Н) g : гравитация (м/с^2) T_sim : общее время симуляции (если None -> берём planner.T_total) N : количество точек для дискретизации max_tilt_deg : максимальный угол крена/тангажа (градусы) """ if T_sim is None: T_sim = planner.T_total a_max = F_MAX / m - g max_tilt = np.deg2rad(max_tilt_deg) print(f"Максимально достижимое ускорение: {a_max:.2f} м/с²") print(f"Максимальный угол наклона: ±{max_tilt_deg}°\n") feasible = True times = np.linspace(0, T_sim, N) for t in times: _, _, acc, yaw = planner.evaluate(t) # Проверка ускорения a_req = np.linalg.norm(acc) if a_req > a_max: print(f"❌ t={t:.2f} c | требуемое a={a_req:.2f} > допустимого {a_max:.2f}") feasible = False # Проверка углов phi_des = np.arcsin( (acc[1]*np.cos(yaw) - acc[0]*np.sin(yaw)) / (acc[2] + g) ) theta_des = np.arctan2( (acc[0]*np.cos(yaw) + acc[1]*np.sin(yaw)), acc[2] + g ) if abs(phi_des) > max_tilt or abs(theta_des) > max_tilt: print(f"⚠️ t={t:.2f} c | угол превышает {max_tilt_deg}° " f"(phi={np.rad2deg(phi_des):.1f}°, theta={np.rad2deg(theta_des):.1f}°)") feasible = False if feasible: print("✅ Вся траектория выполнима!") else: print("\n⚠️ Найдены участки, где траектория нереализуема.") return feasible class MinimumSnapTrajectory: def __init__(self, waypoints, t_points=None, auto_time_estimation=True, simple=False): """ waypoints: n x 3 массив контрольных точек t_points: список времени достижения каждой точки [t0, t1, ..., tn] """ self.waypoints = np.array(waypoints) if auto_time_estimation and t_points is None: # Автоматическая оценка времени t_points = self.auto_estimate_t_points() self.t_points = np.array(t_points) self.n_seg = len(waypoints) - 1 self.dim = 3 # XYZ # Используем полиномы 5-й степени (6 коэффициентов) вместо 7-й self.order = 5 # 5-й степень полинома self.coeffs_per_seg = self.order + 1 # 6 коэффициентов self.coeffs = [] # Список коэффициентов для каждого сегмента и каждой координаты if simple: self._compute_coefficients() else: self._solve_optimization() def _compute_coefficients(self): n = self.n_seg coeffs_per_seg = self.coeffs_per_seg total_coeffs = n * coeffs_per_seg # Создаем матрицу условий A = np.zeros((total_coeffs, total_coeffs)) b = np.zeros((total_coeffs, self.dim)) row = 0 # Условия на позиции в точках соединения for i in range(n + 1): t_seg = 0 if i < n else 1 # 0 для начала сегмента, 1 для конца последнего seg_idx = i if i < n else i - 1 if i < n: # Начало сегмента A[row, seg_idx*coeffs_per_seg:(seg_idx+1)*coeffs_per_seg] = self._poly_vector(0, 0) b[row] = self.waypoints[i] row += 1 if i > 0: # Конец предыдущего сегмента A[row, (i-1)*coeffs_per_seg:i*coeffs_per_seg] = self._poly_vector(1, 0) b[row] = self.waypoints[i] row += 1 # Непрерывность скорости и ускорения в точках соединения for i in range(1, n): T_prev = self.t_points[i] - self.t_points[i-1] T_next = self.t_points[i+1] - self.t_points[i] # Непрерывность скорости (1-я производная) A[row, (i-1)*coeffs_per_seg:i*coeffs_per_seg] = self._poly_vector(1, 1) / T_prev A[row, i*coeffs_per_seg:(i+1)*coeffs_per_seg] = -self._poly_vector(0, 1) / T_next b[row] = 0 row += 1 # Непрерывность ускорения (2-я производная) A[row, (i-1)*coeffs_per_seg:i*coeffs_per_seg] = self._poly_vector(1, 2) / (T_prev**2) A[row, i*coeffs_per_seg:(i+1)*coeffs_per_seg] = -self._poly_vector(0, 2) / (T_next**2) b[row] = 0 row += 1 # Начальные и конечные условия для скорости и ускорения (обнуляем) # Начальные условия T0 = self.t_points[1] - self.t_points[0] A[row, 0:coeffs_per_seg] = self._poly_vector(0, 1) / T0 b[row] = 0 row += 1 A[row, 0:coeffs_per_seg] = self._poly_vector(0, 2) / (T0**2) b[row] = 0 row += 1 # Конечные условия Tn = self.t_points[-1] - self.t_points[-2] A[row, -coeffs_per_seg:] = self._poly_vector(1, 1) / Tn b[row] = 0 row += 1 A[row, -coeffs_per_seg:] = self._poly_vector(1, 2) / (Tn**2) b[row] = 0 row += 1 # Решаем для каждой координаты отдельно self.coeffs = [] for d in range(self.dim): # Используем псевдообратную матрицу для устойчивости alpha = np.linalg.lstsq(A, b[:, d], rcond=None)[0] self.coeffs.append(alpha.reshape((n, coeffs_per_seg))) def _solve_optimization(self): n_seg = self.n_seg n_coeffs = self.order + 1 total_coeffs = n_seg * n_coeffs self.coeffs = [] from scipy.optimize import minimize # Решаем отдельно для каждой координаты (x, y, z) for dim in range(self.dim): A_eq = [] b_eq = [] # Ограничения позиций в waypoints for i in range(n_seg + 1): if i < n_seg: # начало сегмента row = np.zeros(total_coeffs) seg_start = i * n_coeffs row[seg_start:seg_start+n_coeffs] = self._poly_vector(0, 0) A_eq.append(row) b_eq.append(self.waypoints[i, dim]) if i > 0: # конец предыдущего сегмента row = np.zeros(total_coeffs) seg_start = (i-1) * n_coeffs row[seg_start:seg_start+n_coeffs] = self._poly_vector(1, 0) A_eq.append(row) b_eq.append(self.waypoints[i, dim]) # Непрерывность производных (скорость, ускорение, рывок) for i in range(1, n_seg): T_prev = self.t_points[i] - self.t_points[i-1] T_next = self.t_points[i+1] - self.t_points[i] for deriv in range(1, 4): row = np.zeros(total_coeffs) seg_prev = (i-1) * n_coeffs seg_next = i * n_coeffs row[seg_prev:seg_prev+n_coeffs] = self._poly_vector(1, deriv) / (T_prev**deriv) row[seg_next:seg_next+n_coeffs] = -self._poly_vector(0, deriv) / (T_next**deriv) A_eq.append(row) b_eq.append(0.0) # Начальные условия (v=a=j=0) T0 = self.t_points[1] - self.t_points[0] for deriv in range(1, 4): row = np.zeros(total_coeffs) row[0:n_coeffs] = self._poly_vector(0, deriv) / (T0**deriv) A_eq.append(row) b_eq.append(0.0) # Конечные условия (v=a=j=0) Tn = self.t_points[-1] - self.t_points[-2] for deriv in range(1, 4): row = np.zeros(total_coeffs) row[-n_coeffs:] = self._poly_vector(1, deriv) / (Tn**deriv) A_eq.append(row) b_eq.append(0.0) A_eq = np.array(A_eq) b_eq = np.array(b_eq) # Целевая функция (минимизация snap) Q = self._build_snap_hessian(n_seg, n_coeffs) def objective(x): return 0.5 * x.T @ Q @ x def objective_jac(x): return Q @ x constraints = { 'type': 'eq', 'fun': lambda x: A_eq @ x - b_eq, 'jac': lambda x: A_eq } x0 = np.zeros(total_coeffs) result = minimize(objective, x0, jac=objective_jac, constraints=constraints, method='SLSQP') if not result.success: print(f"⚠️ Оптимизация не сошлась для dim={dim}: {result.message}") coeffs_dim = result.x.reshape((n_seg, n_coeffs)) self.coeffs.append(coeffs_dim) def _build_snap_hessian(self, n_seg, n_coeffs): """Строит матрицу Q для минимизации ∫(d⁴p/dt⁴)² dt""" total_coeffs = n_seg * n_coeffs Q = np.zeros((total_coeffs, total_coeffs)) for seg in range(n_seg): T = self.t_points[seg+1] - self.t_points[seg] # FIXED: было segment_times start_idx = seg * n_coeffs # Матрица Гессе для одного сегмента Q_seg = np.zeros((n_coeffs, n_coeffs)) for i in range(n_coeffs): for j in range(n_coeffs): if i >= 4 and j >= 4: # Snap = 4-я производная coeff_i = i*(i-1)*(i-2)*(i-3) coeff_j = j*(j-1)*(j-2)*(j-3) power = i + j - 7 if power != -1: integral = (1**(power+1) - 0**(power+1)) / (power+1) else: integral = np.log(1) - np.log(0) # особый случай Q_seg[i,j] = coeff_i * coeff_j * integral / (T**(i + j -7)) # масштабирование Q[start_idx:start_idx+n_coeffs, start_idx:start_idx+n_coeffs] = Q_seg return Q def _poly_vector(self, tau, derivative_order): """ Возвращает вектор коэффициентов для производной порядка derivative_order """ vec = np.zeros(self.coeffs_per_seg) for i in range(derivative_order, self.coeffs_per_seg): coef = 1 for j in range(i-derivative_order+1, i+1): coef *= j vec[i] = coef * tau**(i-derivative_order) return vec def evaluate(self, t): """ Возвращает pos, vel, acc в момент времени t """ # Найти сегмент if t <= self.t_points[0]: i = 0 tau = 0 elif t >= self.t_points[-1]: i = self.n_seg-1 tau = 1 else: i = np.searchsorted(self.t_points, t) - 1 T = self.t_points[i+1] - self.t_points[i] tau = (t - self.t_points[i]) / T pos = np.zeros(3) vel = np.zeros(3) acc = np.zeros(3) T = self.t_points[i+1] - self.t_points[i] for d in range(3): c = self.coeffs[d][i] # Полином: c[0] + c[1]*tau + c[2]*tau^2 + ... pos[d] = np.polyval(c[::-1], tau) # Производные с учётом масштаба времени der1 = np.polyval(np.polyder(c[::-1]), tau) / T der2 = np.polyval(np.polyder(c[::-1], 2), tau) / (T**2) vel[d] = der1 acc[d] = der2 # yaw вдоль траектории speed_xy = np.linalg.norm(vel[:2]) psi = np.arctan2(vel[1], vel[0]) if speed_xy > 1e-3 else 0.0 return pos, vel, acc, psi def auto_estimate_t_points(self, v_max=4.0, a_max=6.0, safety_margin=1.6): """Автоматическая оценка времени сегментов""" t_points = [0] cumulative_time = 0 for i in range(len(self.waypoints)-1): start = self.waypoints[i] end = self.waypoints[i+1] distance = np.linalg.norm(end - start) # Оценка времени с учетом динамических ограничений if distance > v_max**2 / a_max: t_segment = (distance / v_max + v_max / a_max) * safety_margin else: t_segment = (2 * np.sqrt(distance / a_max)) * safety_margin cumulative_time += t_segment t_points.append(cumulative_time) return t_points def get_trajectory_info(self): """Возвращает информацию о траектории""" total_distance = 0 for i in range(len(self.waypoints)-1): dist = np.linalg.norm(self.waypoints[i+1] - self.waypoints[i]) total_distance += dist avg_speed = total_distance / self.t_points[-1] if self.t_points[-1] > 0 else 0 return { 'total_time': self.t_points[-1], 'total_distance': total_distance, 'average_speed': avg_speed, 'segments': len(self.waypoints) - 1 } def acceleration_to_angles(acc_des, psi_des, g=9.81): # Разбираем желаемое ускорение ax, ay, az = acc_des # Поворот вокруг Z (yaw) sin_psi, cos_psi = np.sin(psi_des), np.cos(psi_des) # Преобразуем ускорение в локальную СК квадрокоптера ax_body = cos_psi * ax + sin_psi * ay ay_body = -sin_psi * ax + cos_psi * ay # Вычисляем pitch и roll theta = np.arctan2(ax_body, az + g) phi = np.arctan2(-ay_body, az + g) return phi, theta class Quadcopter: def __init__(self): self.pos = np.array([0.0, 0.0, 0.0]) self.vel = np.array([0.0, 0.0, 0.0]) self.angles = np.array([0.0, 0.0, 0.0]) self.ang_vel = np.array([0.0, 0.0, 0.0]) self.w1, self.w2, self.w3, self.w4 = (0., 0., 0., 0.) self.I = I self.I_inv = np.linalg.inv(I) # Предвычислить один раз! def update_state(self, F_total, M, dt): phi, theta, psi = self.angles R = self.rotation_matrix(phi, theta, psi) thrust_vector_inertial = R @ np.array([0, 0, F_total]) acc = (thrust_vector_inertial / m) - np.array([0, 0, g]) self.vel += acc * dt self.pos += self.vel * dt p, q, r = self.ang_vel M_gyroscopic = np.cross(self.ang_vel, self.I @ self.ang_vel) ang_acc = self.I_inv @ (M - M_gyroscopic) self.ang_vel += ang_acc * dt # self.angles += self.ang_vel * dt # Матрица преобразования body rates → эйлер-углы T = np.array([ [1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)] ]) self.angles += (T @ self.ang_vel) * dt def rotation_matrix(self, phi, theta, psi): c_phi, s_phi = np.cos(phi), np.sin(phi) c_theta, s_theta = np.cos(theta), np.sin(theta) c_psi, s_psi = np.cos(psi), np.sin(psi) # R_x = np.array([[1, 0, 0], [0, c_phi, -s_phi], [0, s_phi, c_phi]]) # R_y = np.array([[c_theta, 0, s_theta], [0, 1, 0], [-s_theta, 0, c_theta]]) # R_z = np.array([[c_psi, -s_psi, 0], [s_psi, c_psi, 0], [0, 0, 1]]) # return R_z @ R_y @ R_x R_zyx = np.array([[c_psi*c_theta, c_psi*s_theta*s_phi - s_psi*c_phi, c_psi*s_theta*c_phi + s_psi*s_phi], [s_psi*c_theta, s_psi*s_theta*s_phi + c_psi*c_phi, s_psi*s_theta*c_phi - c_psi*s_phi], [-s_theta, c_theta*s_phi, c_theta*c_phi]]) return R_zyx class Controller: def __init__(self,config, dt): # позиционный контур (pos -> vel_des) # для X/Y/Z можно держать разные наборы, здесь задаём отдельно для Z # Присваиваем по индексам Kp_pos_xy = config[0] # Позиционный XY P #Kd_pos_xy = config[1] # Позиционный XY D Kp_pos_z = config[1] # Позиционный Z P Ki_pos_z = config[2] # Позиционный Z I Kp_vel_xy = config[3] # Скоростной XY P Kd_vel_xy = config[4] # Скоростной XY D Kp_vel_z = config[5] # Скоростной Z P Kd_vel_z = config[6] # Скоростной Z D kp_phi = config[7] # Угловой phi P kp_theta = config[8] # Угловой theta P kp_psi = config[9] # Угловой psi P kd_phi = config[10] # Угловой phi D kd_theta = config[11] # Угловой theta D kd_psi = config[12] # Угловой psi D self.int_z = int_z#config[15] # Интегратор Z self.int_z_min = int_z_min#config[16] # Лимит интегратора min self.int_z_max = int_z_max#config[17] # Лимит интегратора max self.alpha = alpha#config[18] # Коэффициент фильтра self.d_filter_alpha = d_filter_alpha#config[19] # Коэффициент фильтра производной #kp_ang = config[12] # Угловой общий P #kd_ang = config[16] # Угловой общий D #self.d_filter_alpha = config[21]#0.9#0.9 self.pos_des_prev= np.zeros(3) # derivative filter state for vel self.prev_vel_err_z = 0.0 self.d_vel_err_z = 0.0 self.prev_vel_err_xy = np.zeros(2) self.d_vel_err_xy = np.zeros(2) self.phi_prev,self.theta_prev,self.psi_prev=np.zeros(3) self.dt = dt def compute_desired_angles(self,acc_des, psi_des): # acc_des - желательное ускорение в инерциальной системе # psi_des - желаемый yaw ax, ay, az = acc_des + np.array([0,0,g]) # компенсируем гравитацию phi_des = np.arcsin(-ay / np.linalg.norm([ax, ay, az])) theta_des = np.arctan2(ax, az) return phi_des, theta_des, psi_des def acc_to_angles(self, acc_des, psi_des): # acc_des = [ax, ay, az] phi_des = (acc_des[0] * np.sin(psi_des) - acc_des[1] * np.cos(psi_des)) / g theta_des = (acc_des[0] * np.cos(psi_des) + acc_des[1] * np.sin(psi_des)) / g return phi_des, theta_des class PIDOptimizer: def __init__(self, trajectory, dt=0.01): self.trajectory = trajectory self.dt = dt def objective_function(self, params): """Оптимизируем ВСЕ 13 параметров""" try: # params теперь содержит ВСЕ 13 параметров controller = Controller(params, self.dt) quad = Quadcopter() total_error = 0 t_list = np.arange(0, self.trajectory.t_points[-1] + self.dt, self.dt) # Для отладки - отслеживаем движение initial_pos = quad.pos.copy() max_movement = 0 positions_actual = [] positions_desired = [] for t in t_list: pos_des, vel_des, acc_des, psi_des = self.trajectory.evaluate(t) F_total, M, controller.pos_des_prev, controller.int_z, \ controller.prev_vel_err_xy, controller.d_vel_err_xy, \ controller.prev_vel_err_z, controller.d_vel_err_z, controller.psi_prev = \ calculate_commands( quad.pos, quad.vel, quad.angles, quad.ang_vel, pos_des, vel_des, acc_des, psi_des, np.zeros(3), controller.pos_des_prev, controller.int_z, controller.prev_vel_err_xy, controller.d_vel_err_xy, controller.prev_vel_err_z, controller.d_vel_err_z, controller.psi_prev, params # ⚠️ Передаем ВСЕ параметры ) # Проверка стабильности ДО обновления if (np.any(np.isnan(quad.pos)) or np.any(np.isinf(quad.pos)) or np.any(np.isnan(F_total)) or np.isinf(F_total) or F_total > 2 * F_MAX or F_total < -F_MAX): print(f"⚠️ Нестабильность при t={t:.2f}") return 1e10 quad.update_state(F_total, M, self.dt) # Отслеживаем движение movement = np.linalg.norm(quad.pos - initial_pos) max_movement = max(max_movement, movement) positions_actual.append(quad.pos.copy()) positions_desired.append(pos_des.copy()) pos_error = np.linalg.norm(pos_des - quad.pos) total_error += pos_error ** 2 mse = total_error / len(t_list) # ⚠️ Штраф за отсутствие движения if max_movement < 0.1: print(f"⚠️ Не двигается! movement={max_movement:.4f}") return 1e10 + (0.1 - max_movement) * 1e6 # ⚠️ Штраф за слишком большое отклонение if max_movement > 50: # Улетел слишком далеко return 1e10 if np.isnan(mse) or np.isinf(mse): return 1e10 print(f"Params: {[f'{p:.2f}' for p in params]} | MSE: {mse:.6f} | Movement: {max_movement:.2f}m") return mse except Exception as e: print(f"❌ Ошибка: {e}") return 1e10 def optimize_LBFGSB(trajectory, initial_params): """Оптимизация ВСЕХ параметров методом Нелдера-Мида""" optimizer = PIDOptimizer(trajectory) # ⚠️ Границы для ВСЕХ 15 параметров (важно для стабильности!) bounds = [ # PID позиционные (9 параметров) (0.1, 5.0), #(0.1, 3.0), # Kp_pos_xy, Kd_pos_xy (0.1, 5.0), (0.01, 1.0), #(0.1, 3.0), # Kp_pos_z, Ki_pos_z, Kd_pos_z (0.1, 5.0), (0.1, 3.0), # Kp_vel_xy, Kd_vel_xy (0.1, 5.0), (0.1, 3.0), # Kp_vel_z, Kd_vel_z # Угловые коэффициенты (6 параметров) - ⚠️ ТЕПЕРЬ ОПТИМИЗИРУЕМ! (2.0, 15.0), (2.0, 15.0), (2.0, 15.0), # kp_phi, kp_theta, kp_psi (0.5, 5.0), (0.5, 5.0), (0.5, 5.0), # kd_phi, kd_theta, kd_psi ] # Используем алгоритм с поддержкой границ result = minimize( optimizer.objective_function, initial_params, method='L-BFGS-B', # Поддерживает границы bounds=bounds, options={ 'maxiter': 300, 'disp': True, 'ftol': 1e-4, 'gtol': 1e-5 } ) return result.x, result.fun if __name__ == "__main__": #dt = 0.01 #T_sim = 50 quad=Quadcopter() config=[Kp_pos_xy, Kp_pos_z, Ki_pos_z, Kp_vel_xy, Kd_vel_xy, Kp_vel_z, Kd_vel_z, kp_phi, kp_theta, kp_psi,#kp_ang, kd_phi, kd_theta, kd_psi]#,#kd_ang, # anti-windup и фильтр #int_z,int_z_min,int_z_max, #alpha,d_filter_alpha] controller = Controller(config,dt) planner = MinimumSnapTrajectory(waypoints)#,t_points) planner0=copy.deepcopy(planner) if T_sim is None: T_sim=planner.t_points[-1] print (f"Estimated T_sim: {T_sim}") t_list = np.arange(0, T_sim+dt, dt) check_trajectory_feasibility(planner,T_sim=T_sim) #planner = TrajectoryPlanner(waypoints,T_sim) #trajectory=[] for t in np.linspace(0, T_sim+dt, len(waypoints)*3-1): pos, vel, acc, yaw = planner.evaluate(t) #trajectory.append([t,pos, vel, acc, yaw]) print(f"t={t:.2f} | pos={pos} | vel={vel} | acc={acc} | yaw={yaw:.2f}") step_count = 0 ang_vel_des = np.array([0.0, 0.0, 0.0]) np.set_printoptions(precision=2, floatmode='fixed') for t in t_list: pos_des, vel_des, acc_des, psi_des = planner.evaluate(t) ang_vel_des = np.array([0.0, 0.0, 0.0]) F_total, M,controller.pos_des_prev,controller.int_z,controller.prev_vel_err_xy,controller.d_vel_err_xy,controller.prev_vel_err_z,controller.d_vel_err_z,controller.psi_prev = calculate_commands(quad.pos,quad.vel,quad.angles,quad.ang_vel, pos_des, vel_des, acc_des, psi_des, ang_vel_des,controller.pos_des_prev,controller.int_z,controller.prev_vel_err_xy,controller.d_vel_err_xy,controller.prev_vel_err_z,controller.d_vel_err_z,controller.psi_prev,config) quad.w1, quad.w2, quad.w3, quad.w4 = calculate_motor_speeds(F_total, M) quad.update_state(F_total, M, dt) if step_count % 50 == 0: print(f"Time: {t:.2f} s | Current pos: {quad.pos} | Desired pos: {pos_des} | W1..W4 {quad.w1:.2f} {quad.w2:.2f} {quad.w3:.2f} {quad.w4:.2f}") step_count += 1 # print(f"initial config:\n{config}") # x,fun=optimize_LBFGSB(planner0,config) # print(x) # print(fun) # initial_params = [0.5, 0.5, 0.5, 0.05, 0.5, 1.0, 0.5, 1.0, 0.5]#,config[9:20] # x,fun=optimize_LBFGSB(planner0,initial_params+config[9:] ) # print(x) # print(fun) """ Optimization terminated successfully. Current function value: 0.004867 Iterations: 19 Function evaluations: 208 [1.20 1.75 1.50 0.10 0.50 2.50 0.80 3.00 0.50] 0.004866901765047811 """ #initial params: #[1.2, 1.75, 1.5, 0.1, 0.5, 2.5, 0.8, 3.0, 0.5]