/
Logrus
/
CopterControl
Обзор
Документация
Войти
/
Logrus
/
CopterControl
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
CI/CD
Аналитика
Безопасность
master
jitfunc.py
330 строк
13 KB
Nikolay Nosorev
Initial commit
04 окт 2025, 16:17
04 окт 2025, 16:17
99ef28c
Код
Авторство
О чём код?
import numpy as np import math import numpy as np from numba import njit from constants import * @njit(cache=True) def clip_array(x, min_val, max_val): out = np.empty_like(x) for i in range(x.shape[0]): if x[i] < min_val: out[i] = min_val elif x[i] > max_val: out[i] = max_val else: out[i] = x[i] return out # @njit(cache=True) def calculate_motor_speeds(F_total, M): # Разбиваем моменты Mx, My, Mz = M # Прямое решение системы через заранее вычисленные формулы w1_sq = 0.25 / k * F_total - 0.5 / (k * l) * My + 0.25 / b * Mz w2_sq = 0.25 / k * F_total + 0.5 / (k * l) * Mx - 0.25 / b * Mz w3_sq = 0.25 / k * F_total + 0.5 / (k * l) * My + 0.25 / b * Mz w4_sq = 0.25 / k * F_total - 0.5 / (k * l) * Mx - 0.25 / b * Mz # Ограничиваем снизу нулём w1_sq = max(w1_sq, 0.0) w2_sq = max(w2_sq, 0.0) w3_sq = max(w3_sq, 0.0) w4_sq = max(w4_sq, 0.0) # Вычисляем обороты w1 = np.sqrt(w1_sq) w2 = np.sqrt(w2_sq) w3 = np.sqrt(w3_sq) w4 = np.sqrt(w4_sq) return w1, w2, w3, w4 @njit(cache=True) def calculate_commands( quad_pos,quad_vel,quad_angles,quad_ang_vel, pos_des, vel_des_traj, acc_des_traj, psi_des, ang_vel_des,pos_des_prev,int_z,prev_vel_err_xy,d_vel_err_xy,prev_vel_err_z,d_vel_err_z,psi_prev,config): 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 #int_z = config[15] # Интегратор Z #int_z_min = config[16] # Лимит интегратора min #int_z_max = config[17] # Лимит интегратора max #alpha = config[18] # Коэффициент фильтра #d_filter_alpha = config[19] # Коэффициент фильтра производной #kp_ang = config[12] # Угловой общий P #kd_ang = config[16] # Угловой общий D # ---------- POSITION -> velocity setpoint ---------- pos_des_filtered_x = alpha * pos_des_prev[0] + (1-alpha) * pos_des[0] pos_des_filtered_y = alpha * pos_des_prev[1] + (1-alpha) * pos_des[1] pos_des_filtered_z = alpha * pos_des_prev[2] + (1-alpha) * pos_des[2] pos_err_x = pos_des_filtered_x - quad_pos[0] pos_err_y = pos_des_filtered_y - quad_pos[1] pos_err_z = pos_des_filtered_z - quad_pos[2] pos_des_prev[0] = pos_des[0] pos_des_prev[1] = pos_des[1] pos_des_prev[2] = pos_des[2] # X/Y: простое P vel_des_cmd_x = vel_des_traj[0] + Kp_pos_xy * pos_err_x vel_des_cmd_y = vel_des_traj[1] + Kp_pos_xy * pos_err_y # Z: PID vz_traj = vel_des_traj[2] int_z += Ki_pos_z * pos_err_z * dt if int_z > int_z_max: int_z = int_z_max elif int_z < int_z_min: int_z = int_z_min vz_des = vz_traj + Kp_pos_z * pos_err_z + int_z if vz_des > 5.0: vz_des = 5.0 elif vz_des < -5.0: vz_des = -5.0 # ---------- VELOCITY -> acceleration command ---------- vel_err_x = vel_des_cmd_x - quad_vel[0] vel_err_y = vel_des_cmd_y - quad_vel[1] vel_err_z = vz_des - quad_vel[2] # XY: PD с фильтрацией d_vel_err_xy_raw_x = (vel_err_x - prev_vel_err_xy[0]) / dt d_vel_err_xy_raw_y = (vel_err_y - prev_vel_err_xy[1]) / dt d_vel_err_xy[0] = d_filter_alpha * d_vel_err_xy[0] + (1 - d_filter_alpha) * d_vel_err_xy_raw_x d_vel_err_xy[1] = d_filter_alpha * d_vel_err_xy[1] + (1 - d_filter_alpha) * d_vel_err_xy_raw_y prev_vel_err_xy[0] = vel_err_x prev_vel_err_xy[1] = vel_err_y acc_cmd_x = acc_des_traj[0] + Kp_vel_xy * vel_err_x + Kd_vel_xy * d_vel_err_xy[0] acc_cmd_y = acc_des_traj[1] + Kp_vel_xy * vel_err_y + Kd_vel_xy * d_vel_err_xy[1] # Z: PD с фильтрацией d_vel_err_z_raw = (vel_err_z - prev_vel_err_z) / dt d_vel_err_z = d_filter_alpha * d_vel_err_z + (1 - d_filter_alpha) * d_vel_err_z_raw acc_cmd_z = acc_des_traj[2] + Kp_vel_z * vel_err_z + Kd_vel_z * d_vel_err_z prev_vel_err_z = vel_err_z # Итоговый вектор ускорений acc_cmd_x = max(min(acc_cmd_x, ACC_MAX), -ACC_MAX) acc_cmd_y = max(min(acc_cmd_y, ACC_MAX), -ACC_MAX) acc_cmd_z = max(min(acc_cmd_z, ACC_MAX), -ACC_MAX) # ---------- convert acc_cmd -> desired angles and thrust ---------- acc_total = math.sqrt((acc_cmd_x**2 + acc_cmd_y**2 + (acc_cmd_z + g)**2)) if acc_total < 1e-3: acc_total = 1e-3 # Желаемое направление тяги thrust_x = acc_cmd_x #/ acc_total thrust_y = acc_cmd_y #/ acc_total thrust_z = (acc_cmd_z + g) #/ acc_total cos_psi = math.cos(psi_des) sin_psi = math.sin(psi_des) x_body = (cos_psi * thrust_x + sin_psi * thrust_y)/acc_total y_body = (-sin_psi * thrust_x + cos_psi * thrust_y)/acc_total z_body = (thrust_z)/acc_total #theta_des = math.asin(max(min(x_body / acc_total, 1.0), -1.0)) theta_des = math.asin(max(min(x_body , 1.0), -1.0)) phi_des = math.atan2(-y_body, z_body) phi_des = max(min(phi_des, math.radians(45.0)), -math.radians(45.0)) theta_des = max(min(theta_des, math.radians(45.0)), -math.radians(45.0)) F_total = m * acc_total F_total = max(min(F_total, F_MAX), F_MIN) # ---------- attitude control ---------- phi, theta, psi = quad_angles p, q, r = quad_ang_vel p_des = 0.0 q_des = 0.0 r_des = 0.0 phi_prev = phi_des theta_prev = theta_des psi_prev = psi_des ex = phi_des - phi ey = theta_des - theta ez = psi_des - psi evx = p_des - p evy = q_des - q evz = r_des - r Mx = kp_phi * ex + kd_phi * evx My = kp_theta * ey + kd_theta * evy Mz = kp_psi * ez + kd_psi * evz return F_total, (Mx, My, Mz), pos_des_prev, int_z, prev_vel_err_xy, d_vel_err_xy, prev_vel_err_z, d_vel_err_z, psi_prev @njit(cache=True) def calculate_commands0( quad_pos,quad_vel,quad_angles,quad_ang_vel, pos_des, vel_des_traj, acc_des_traj, psi_des, ang_vel_des,pos_des_prev,int_z,prev_vel_err_xy,d_vel_err_xy,prev_vel_err_z,d_vel_err_z,psi_prev,config): 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 #int_z = config[13] # Интегратор Z #int_z_min = config[14] # Лимит интегратора min #int_z_max = config[15] # Лимит интегратора max #alpha = config[16] # Коэффициент фильтра #d_filter_alpha = config[17] # Коэффициент фильтра производной #kp_ang = config[12] # Угловой общий P #kd_ang = config[16] # Угловой общий D # ---------- POSITION -> velocity setpoint ---------- # if 'pos_des_prev' not in locals(): # pos_des_prev = pos_des pos_des_filtered = alpha * pos_des_prev + (1-alpha) * pos_des #pos_err = pos_des_filtered - quad.pos pos_err = pos_des_filtered - quad_pos pos_des_prev = pos_des # X/Y: простое P vel_des_cmd_xy = vel_des_traj[:2] + Kp_pos_xy * pos_err[:2] # Z: PID (pos -> desired vz) vz_traj = vel_des_traj[2] pos_err_z = pos_err[2] # интегратор int_z += Ki_pos_z * pos_err_z * dt int_z = min(max(int_z, int_z_min), int_z_max)#np.clip(int_z, int_z_min, int_z_max) vz_des = vz_traj + Kp_pos_z * pos_err_z + int_z vz_des = min(max(vz_des, -5.0), 5.0)#np.clip(vz_des, -5.0, 5.0) vel_des_cmd = np.array([vel_des_cmd_xy[0], vel_des_cmd_xy[1], vz_des]) # ---------- VELOCITY -> acceleration command ---------- vel_err = vel_des_cmd - quad_vel # XY: PD с фильтрацией производной vel_err_xy = vel_err[:2] d_vel_err_xy_raw = (vel_err_xy - prev_vel_err_xy) / dt d_vel_err_xy = d_filter_alpha * d_vel_err_xy + (1 - d_filter_alpha) * d_vel_err_xy_raw #raw_d_xy = (vel_err[:2] - prev_vel_err_xy) / dt #d_vel_err_xy = d_filter_alpha * d_vel_err_xy + (1 - d_filter_alpha) * raw_d_xy #prev_vel_err_xy = vel_err[:2] acc_cmd_xy = acc_des_traj[:2] \ + Kp_vel_xy * vel_err[:2] \ + Kd_vel_xy * d_vel_err_xy prev_vel_err_xy = vel_err_xy.copy() # Z: PD с фильтрацией vel_err_z = vel_err[2] d_vel_err_z_raw = (vel_err_z - prev_vel_err_z) / dt d_vel_err_z = d_filter_alpha * d_vel_err_z + (1 - d_filter_alpha) * d_vel_err_z_raw acc_cmd_z = acc_des_traj[2] + Kp_vel_z * vel_err_z + Kd_vel_z * d_vel_err_z prev_vel_err_z = vel_err_z # итоговый вектор ускорений acc_cmd = np.array([acc_cmd_xy[0], acc_cmd_xy[1], acc_cmd_z]) acc_cmd = acc_cmd = clip_array(acc_cmd, -ACC_MAX, ACC_MAX)#min(max(acc_cmd, -ACC_MAX), ACC_MAX) #np.clip(acc_cmd, -ACC_MAX, ACC_MAX) # ---------- convert acc_cmd -> desired angles and thrust ---------- # Правильное преобразование ускорения в углы acc_total = np.linalg.norm(acc_cmd + np.array([0, 0, g])) if acc_total < 1e-3: acc_total = 1e-3 # Желаемое направление тяги в инерциальной системе thrust_direction = (acc_cmd + np.array([0, 0, g])) / acc_total # Преобразуем в углы с учетом рыскания cos_psi, sin_psi = np.cos(psi_des), np.sin(psi_des) # Компоненты в телевой системе после поворота на рыскание x_body = cos_psi * thrust_direction[0] + sin_psi * thrust_direction[1] y_body = -sin_psi * thrust_direction[0] + cos_psi * thrust_direction[1] z_body = thrust_direction[2] # Вычисляем углы крена и тангажа theta_des = np.arcsin(min(max(x_body, -1.0), 1.0))#np.arcsin(np.clip(x_body, -1.0, 1.0)) phi_des = np.arctan2(-y_body, z_body) # Ограничиваем углы phi_des = min(max(phi_des, -np.deg2rad(45)), np.deg2rad(45))#np.clip(phi_des, -np.deg2rad(45), np.deg2rad(45)) theta_des = min(max(theta_des, -np.deg2rad(45)), np.deg2rad(45))#np.clip(theta_des, -np.deg2rad(45), np.deg2rad(45)) # Общая тяга F_total = m * acc_total F_total = min(max(F_total, F_MIN), F_MAX)#np.clip(F_total, F_MIN, F_MAX) # ---------- attitude control ---------- phi, theta, psi = quad_angles p, q, r = quad_ang_vel # желаемые угловые скорости для phi/theta по ускорениям p_des = 0#(theta_des - theta_prev)/dt # можно добавить, если есть модель акселераций q_des = 0#(phi_des - phi_prev)/dt r_des = 0#(psi_des - psi_prev)/dt phi_prev,theta_prev,psi_prev=phi_des,theta_des,psi_des #Kp_ang = np.array([kp_phi, kp_theta, kp_psi]) #Kd_ang = np.array([kd_phi, kd_theta, kd_psi]) #e_angles = np.array([phi_des - phi, theta_des - theta, psi_des - psi]) #e_ang_vel = np.array([p_des - p, q_des - q, r_des - r]) # пока без feedforward #M = Kp_ang * e_angles + Kd_ang * e_ang_vel ex,ey,ez = phi_des - phi,theta_des - theta,psi_des - psi evx,evy,evz = p_des - p,q_des - q,r_des - r Mx = kp_phi * ex + kd_phi * evx My = kp_theta * ey + kd_theta * evy Mz = kp_psi * ez + kd_psi * evz M = (Mx, My, Mz) return F_total, M,pos_des_prev, int_z,prev_vel_err_xy,d_vel_err_xy,prev_vel_err_z,d_vel_err_z,psi_prev from numba import njit # @njit(cache=True) # def calculate_motor_speeds(F_total, M): # A = np.array([ # [k, k, k, k], # [0, -l*k, 0, l*k], # [-l*k, 0, l*k, 0], # [b, -b, b, -b] # ]) # # Предвычисляем обратную один раз # #A_inv = np.linalg.inv(A) # A_inv=np.array([ # [0.25/k, 0, -1/(2*k*l), 0.25/b], # [0.25/k, 0.5/(k*l), 0, -0.25/b], # [0.25/k, 0, 1/(2*k*l), 0.25/b], # [0.25/k, -0.5/(k*l), 0, -0.25/b] # ]) # b_vec = np.array([F_total, M[0], M[1], M[2]]) # # Решаем напрямую через предвычисленную A_inv # omega_sq = A_inv.dot(b_vec) # # Ограничиваем снизу нулем (нельзя отрицательные обороты) # omega_sq = np.maximum(omega_sq, 0.0) # w1, w2, w3, w4 = np.sqrt(omega_sq) # return w1, w2, w3, w4