/
fedorovaaa2
/
ucheba_python
Обзор
Документация
Войти
/
fedorovaaa2
/
ucheba_python
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
main
satellite_model/src/mission_simulation.py
790 строк
22 KB
Zaynulla Zhumaev
* sun, ballistic: Angle deduplication;
24 сен 2023, 19:02
24 сен 2023, 19:02
6a818dd
Код
Авторство
О чём код?
"""Spacecraft ODEs integration""" # %% import math import pickle from datetime import datetime from pathlib import Path import numpy as np from pyquaternion import Quaternion from scipy.integrate import solve_ivp from adcs import ( WHEELS_MAT, a_io, command_quaternion, error_quaternion, omega_mat, wheels_acceleration, ) from ballistic import ( free_flight_phasing, free_flight_simulation, phasing_angle, sat_is_in_thrusting_allowed_zone, satellite_acceleration, satellite_initial_orbit_parameters, sun_vector, sun_visibility, thrusting_allowed_event, ) from cstpu import ( EPSILON, SIGMA, SUN_CONSTANT, VALVE_OPENING_TEMPERATURE, calc_water_steam_equilibrium, calculate_flow_from_nozzle, determine_valve_output, internal_energy_from_temperature, steam_enthalpy_mass_flow_product, ) from mission_parameters import ( ALPHA_GOAL, ANGLE_ACCURACY, BATTERY_CAPACITY, CONCENTRATOR_EFFICIENCY, CONCENTRATOR_MIN_PROJECTION, CONCENTRATOR_SIZE_X, CONCENTRATOR_SIZE_Y, EQUIVALENT_SQUARE, FINAL_FUEL_MASS, J_SAT, K_D, K_P, M_FUEL_0, MACHINE_ZERO, MANEUVERING_MAX_DURATION, MISSION_PREFIX, OMEGA_0, OMEGA_ACCURACY, POWER_CONSUMPTION, QUATERNION_0, RESULTS_FOLDER, SOL_GOAL_B, T_INI_K, THRUST_OFFSET, THRUST_UNIT_V, VALVE_OPENING_DURATION, WHEEL_INERTIA, WHEEL_INI_RATE, ) from power_budget import power_income # %% DEBUG_PRINT = True # %% def heat_flux_from_sun(sun_visible_part, e_sun_b): """Simulates on orbit heat flux from Sun.""" # TODO consider Sun angle in body frame # acos(0.98) is around 11 degrees if (sun_visible_part > 0) and np.dot( e_sun_b, SOL_GOAL_B ) > CONCENTRATOR_MIN_PROJECTION: res = ( sun_visible_part * SUN_CONSTANT * CONCENTRATOR_SIZE_X * CONCENTRATOR_SIZE_Y * CONCENTRATOR_EFFICIENCY ) else: res = 0 return res def total_radiative_heat_flux(current_time, temperature, r_earth_sat, q_bi): """Computes total heat radiative heat flux from Sun and heat dissipation through Stephan - Boltzmann low. Parameters ---------- current_time : float Current simulation time, second. temperature : float Temperature of spacecraft construction and water-steam mixture, K. r_earth_sat : 1x3 array like of float Satellite position in ECI frame, m. q_bi : Quaternion object Rotation quaternion from inertial to body frame. Returns ------- float Sum of heat flux from Sun and radiative dissipation heat flux, W. """ e_sun_eci = sun_vector(current_time) e_sun_b = q_bi.rotate(e_sun_eci) total_flux = ( heat_flux_from_sun(sun_visibility(current_time, r_earth_sat), e_sun_b) - SIGMA * EQUIVALENT_SQUARE * EPSILON * temperature**4 ) return total_flux def total_mixture_energy_derivative(cur_time, temperature, mass_flow, r_sat, q_bi): """Computes water-steam mixture total energy derivative as a total radiative heat flux minus energy losses together with released steam. Parameters ---------- current_time : float Current simulation time, second. temperature : float Temperature of spacecraft construction and water-steam mixture, K. mass_flow : float mass flow rate, kg/second. r_sat : 1x3 array like of float satellite position in ECI, m. q_bi : Quaternion object rotation quaternion from inertial to body frame. Returns ------- float Energy derivative, Watt. """ energy_derivative = total_radiative_heat_flux( cur_time, temperature, r_sat, q_bi ) - steam_enthalpy_mass_flow_product(temperature, mass_flow) return energy_derivative # %% def thrusting_finished_event(t, y, valve_last_time_opened, sat_state, strategy): res = (t - valve_last_time_opened) - VALVE_OPENING_DURATION return res def eclipse_event(t, y, valve_last_time_opened, sat_state, strategy): r_vector = y[2:5] sun_vis = sun_visibility(t, r_vector) res = sun_vis - 0.5 return res def heat_up_event(t, y, valve_last_time_opened, sat_state, strategy): t_real_k = calc_water_steam_equilibrium(y[0], y[1])[1] res = t_real_k - VALVE_OPENING_TEMPERATURE return res def orientation_event(t, y, valve_last_time_opened, sat_state, strategy): omega_bi_b = y[8:11] q_bi = Quaternion(y[11:15]) quat_cmd = command_quaternion(t, y, sat_state, strategy=strategy) q_err = q_bi * quat_cmd.inverse res = ( np.linalg.norm(omega_bi_b) + abs(q_err.angle) - ANGLE_ACCURACY - OMEGA_ACCURACY ) return res def hypervisor(t, y, valve_last_time_opened, cur_sat_state, strategy): """Determines new satellite state. Parameters ---------- t : float time, second. y : solution vector of arrays for each parameter [[internal_energy], [m_sum], ... ] valve_last_time_opened : float Last time when valve was opened. cur_sat_state : string current satellite state in "eclipse", "orbital_orientation", etc. Returns ------- string new satellite state in "eclipse", "orbital_orientation", etc. """ m_sum = y[1] # t_real_k = calc_water_steam_equilibrium(y[0], y[1])[1] r_vector = y[2:5] event_functions = [ thrusting_finished_event, eclipse_event, heat_up_event, orientation_event, thrusting_allowed_event, ] for event_func in event_functions: event_func.terminal = False event_func.direction = 0 sun_vis = sun_visibility(t, r_vector) if cur_sat_state == "initial": if sun_vis > 0.5: new_sat_state = "heat_accumulation" else: new_sat_state = "eclipse" elif cur_sat_state == "heat_accumulation": if abs(sun_vis - 0.5) < MACHINE_ZERO: new_sat_state = "eclipse" else: if sat_is_in_thrusting_allowed_zone(y, strategy): new_sat_state = "orbital_orientation" else: new_sat_state = "waiting_thrusting_allowed_zone" elif cur_sat_state in ( "orbital_orientation", "waiting_thrusting_allowed_zone", ): new_sat_state = "thrusting" elif cur_sat_state == "eclipse": new_sat_state = "heat_accumulation" elif cur_sat_state == "thrusting": if m_sum > FINAL_FUEL_MASS: new_sat_state = "eclipse" if sun_vis < 0.5 else "heat_accumulation" else: new_sat_state = "out_of_fuel" elif cur_sat_state == "out_of_fuel": new_sat_state = cur_sat_state # to avoid error at simulation end else: raise Exception("Unknown satellite state") if new_sat_state == "heat_accumulation": eclipse_event.terminal = True heat_up_event.terminal = True eclipse_event.direction = -1 elif new_sat_state == "eclipse": eclipse_event.terminal = True eclipse_event.direction = 1 elif new_sat_state == "waiting_thrusting_allowed_zone": thrusting_allowed_event.terminal = True elif new_sat_state == "thrusting": thrusting_finished_event.terminal = True elif new_sat_state == "orbital_orientation": orientation_event.terminal = True events = [event_f for event_f in event_functions if event_f.terminal] return new_sat_state, events # %% def f_t_y(t, y, valve_last_time_opened, sat_state, strategy): """Right side of ODE dy/dt = f_t_y. Parameters ---------- t : float time, second. y : solution vector of arrays for each parameter [internal_energy, m_sum, x1, x2, x3, v1, v2, v3, omega_bi_b1, omega_bi_b2, omega_bi_b3, q_bi0 - scalar part of quaternion, q_bi1 - vector part of quaternion, q_bi2 - vector part of quaternion, q_bi3 - vector part of quaternion, wheel0_rate, wheel1_rate, wheel2_rate, wheel3_rate, acc_power - accumulated electrical power, W * sec ] valve_last_time_opened : float Last time when valve was opened sat_state : string satellite state in "eclipse", "orbital_orientation", etc. Returns ------- f_t_y : array of float f_t_y[0] - total radiative heat flux, W. f_t_y[1] - m_dot mass flow, kg/sec. f_t_y[2:5] - velocity vector components, m/s f_t_y[5:8] - acceleration vector components, m/s^2 f_t_y[8:11] - omega_bi_b derivative, s^-2 f_t_y[11:15] - q_bi quaternion derivative, [rad, m, m, m] f_t_y[15:19] - wheels_acceleration, s^-2 f_t_y[19] - accumulated electrical power derivative, W """ internal_energy, m_fuel = y[0], y[1] r_vector = y[2:5] # TODO why conversion to list only in one line v_vector = list(y[5:8]) omega_bi_b = y[8:11] q_bi = Quaternion(y[11:15]) wheels_rate = y[15:19] acc_power = y[19] p_c_bar, t_real_k = calc_water_steam_equilibrium(internal_energy, m_fuel)[0:2] # thrust, imp_sp, mdot = calculate_flow_from_nozzle(p_c_bar, t_real_k) valve_is_opened = sat_state == "thrusting" p_after_valve_bar, t_after_valve_k = determine_valve_output( p_c_bar, t_real_k, valve_is_opened ) thrust, _, mass_flow = calculate_flow_from_nozzle( p_after_valve_bar, t_after_valve_k ) m_dot = -mass_flow E_dot = total_mixture_energy_derivative(t, t_real_k, mass_flow, r_vector, q_bi) q_ib = q_bi.inverse thrust_vector = q_ib.rotate(thrust * THRUST_UNIT_V) a_vector = satellite_acceleration(r_vector, thrust_vector, m_fuel, v_vector) j_sat_inv = np.linalg.inv(J_SAT) # TODO change torque to cmd + external quat_cmd = command_quaternion(t, y, sat_state, strategy=strategy) quat_err = error_quaternion(q_bi, quat_cmd) torque_cmd = -1 * np.dot(K_D[sat_state], omega_bi_b) - np.dot( K_P[sat_state], quat_err.vector ) rw_acc = wheels_acceleration(torque_cmd, wheels_rate) torque_rw_list = -WHEEL_INERTIA * rw_acc torque_rw = np.dot(WHEELS_MAT, torque_rw_list) # TODO consider real external torques torques_ext = np.array([0, 0, 0]) torque_thrust = np.cross(THRUST_OFFSET, thrust * THRUST_UNIT_V) torques_sum = torques_ext + torque_rw + torque_thrust omega_bi_b_dot = np.dot( j_sat_inv, (torques_sum - np.cross(omega_bi_b, np.dot(J_SAT, omega_bi_b))), ) # Euler’s rotational equation, 3.81 equation from # F. Landis Markley, John L. Crassidis # Fundamentals of Spacecraft Attitude Determination and Control q_bi_dot = 0.5 * np.dot(omega_mat(omega_bi_b), q_bi.elements) e_sun_eci = sun_vector(t) e_sun_b = q_bi.rotate(e_sun_eci) electric_power = ( power_income(sun_visibility(t, r_vector), e_sun_b) - POWER_CONSUMPTION[sat_state] ) if acc_power >= BATTERY_CAPACITY and electric_power > 0: electric_power = 0 elif acc_power <= 0 and electric_power < 0: electric_power = 0 res = np.concatenate( ( E_dot, m_dot, v_vector, a_vector, omega_bi_b_dot, q_bi_dot, rw_acc, electric_power, ), axis=None, ) return res def debug_print(y, t_span, sat_state, strategy): """Prints intermediate values and state. Parameters ---------- y : 1xn array state vector at the beginning of integration t_span : 1x2 array start and finish time of simulation sat_state : string satellite state from "eclipse", "heat_accumulation", etc. """ internal_energy, m_fuel = y[0], y[1] r_vector = y[2:5] t_real_k = calc_water_steam_equilibrium(internal_energy, m_fuel)[1] t = t_span[0] sun_vis = sun_visibility(t, r_vector) print(f"\n\n{t_span=}") print(f"Mission: {MISSION_PREFIX}") print(f"Strategy: {strategy}") print(f"{sat_state=}") print(f"Sun visibility: {sun_vis}") print(f"Temperature: {t_real_k}, K") print(f"Fuel mass: {m_fuel}, kg") cur_os_time = datetime.now() print(f"Current OS time: {cur_os_time}") # %% def simulation_launch( t0, t_end, final_fuel_mass, y0=None, strategy="increase periapsis" ): """Function to simulate all satellite subsystems Args: t0 (float): initial time t_end (float): end of simulation time final_fuel_mass (float): lower limit of fuel mass at which the simulation ends y0 (list, optional): initial parameters vector if need to continue previous calculations. Defaults to None. strategy (str, optional): Strategy by satellite make elliptic orbit more circular. One of "increase periapsis" or "decrease apoapsis". Defaults to "increase periapsis". Returns: list: [t, y, sat_states]. List of time moments, corresponding state vectors, and satellite states from "thrusting", "eclipse", "heat accumulation", etc. """ t_span = [t0, t_end] if y0 is None: y0_0 = internal_energy_from_temperature(M_FUEL_0, T_INI_K) y1_0 = M_FUEL_0 y2_8_0 = satellite_initial_orbit_parameters() y_8_11_0 = OMEGA_0 y_11_15_0 = QUATERNION_0.elements y_15_19_0 = WHEEL_INI_RATE y_19_0 = BATTERY_CAPACITY y0 = np.concatenate( (y0_0, y1_0, y2_8_0, y_8_11_0, y_11_15_0, y_15_19_0, y_19_0), axis=None, ).tolist() # TODO is converting to list really necessary? # y0 = list(y0) t = np.array([0]) y = [[elem] for elem in y0] valve_last_time_opened = t0 m_fuel = y0[1] sat_state, events = hypervisor( t[-1], y0, valve_last_time_opened, "initial", strategy ) sat_states = [sat_state] # TODO возможно убрать ограничение по времени, оставить только по топливу while t_span[0] < (t_end - MACHINE_ZERO) and (m_fuel > final_fuel_mass): if sat_state == "thrusting": max_step = VALVE_OPENING_DURATION / 10 valve_last_time_opened = t[-1] else: max_step = 100 # TODO define global constant for max step if DEBUG_PRINT: debug_print(y0, t_span, sat_state, strategy) sol_next_steps = solve_ivp( f_t_y, t_span, y0, method="Radau", args=(valve_last_time_opened, sat_state, strategy), events=events, max_step=max_step, rtol=1e-4, ) y0 = [elem[-1] for elem in sol_next_steps.y] m_fuel = y0[1] t_span = [sol_next_steps.t[-1], t_end] t = np.append(t, sol_next_steps.t) y = np.append(y, sol_next_steps.y, axis=1) sat_states = np.append(sat_states, [sat_state] * len(sol_next_steps.t)) y_last = [elem[-1] for elem in y] sat_state, events = hypervisor( t[-1], y_last, valve_last_time_opened, sat_state, strategy ) return [t, y, sat_states] # %% def two_satellites_maneuvering(t0, t_end, final_fuel_mass, y0_matrix, strategies): if DEBUG_PRINT: print(f"\n\n{'=' * 40}\nFirst satellite maneuvering\n\n") sat1_sol = simulation_launch( t0, t_end, final_fuel_mass, y0_matrix[0], strategies[0] ) if DEBUG_PRINT: print(f"\n\n{'=' * 40}\nSecond satellite maneuvering\n\n") sat2_sol = simulation_launch( t0, t_end, final_fuel_mass, y0_matrix[1], strategies[1] ) sat_solutions = [sat1_sol, sat2_sol] sat_y_last_points = [sol[1][:, -1] for sol in sat_solutions] last_t_moments = [sol[0][-1] for sol in sat_solutions] # sat1_y_last_point = sat1_sol[1][:, -1] # sat2_y_last_point = sat2_sol[1][:, -1] # t1_last = sat1_sol[0][-1] # t2_last = sat2_sol[0][-1] t0_single_free = min(last_t_moments) t_end_single_free = max(last_t_moments) r_last_points = [y[2:5] for y in sat_y_last_points] v_last_points = [y[5:8] for y in sat_y_last_points] # r1 = sat1_y_last_point[2:5] # v1 = sat1_y_last_point[5:8] # r2 = sat2_y_last_point[2:5] # v2 = sat2_y_last_point[5:8] # simulate single satellite free flight to reach equal time sat_min_index = 0 if last_t_moments[0] < last_t_moments[1] else 1 y0_free = np.concatenate( (r_last_points[sat_min_index], v_last_points[sat_min_index]), axis=0 ) free_flight_single_sol = free_flight_simulation( y0_free, t0_single_free, t_end_single_free ) # free_flight_single_sol.satellite_num = sat_min_index r_last_points[sat_min_index] = free_flight_single_sol.y[0:3][:, -1] v_last_points[sat_min_index] = free_flight_single_sol.y[3:6][:, -1] sat_y_last_points[sat_min_index][2:5] = r_last_points[sat_min_index] sat_y_last_points[sat_min_index][5:8] = v_last_points[sat_min_index] return ( sat_solutions, free_flight_single_sol, sat_y_last_points, ) # %% def define_other_parameters(sol): """Define all other parameters of the model in all time stamps from parameters used in model simulation. Parameters ---------- sol : array t : array of float time, second. y : solution vector of arrays for each parameter [[internal_energy], [m_sum], [x1], [x2], [x3], [v1], [v2], [v3], ] sat_states : array of string Returns ------- res : vector of arrays """ # FIXME change it, send strategy as a parameter strategy = "increase periapsis" t = sol[0] y = sol[1] internal_energy, m_sum, *_ = y omega_bi_b = np.array(y[8:11]).T quat_array = np.array(y[11:15]).T quat_obj = [Quaternion(elem) for elem in quat_array] r_vector = np.array(y[2:5]).T v_vector = np.array(y[5:8]).T sat_states = sol[2] wheels_rate = np.array(sol[1][15:19]).T v_tank = np.vectorize(calc_water_steam_equilibrium, otypes=[float] * 4) v_valve_output = np.vectorize(determine_valve_output, otypes=[float] * 2) v_nozzle = np.vectorize(calculate_flow_from_nozzle, otypes=[float] * 3) # v_sun_vector = np.vectorize(sun_vector, otypes=[float] * 3) valve_states = [(elem == "thrusting") for elem in sat_states] p_chamber, t_chamber, m_steam, m_liq = v_tank(internal_energy, m_sum) p_nozzle, t_nozzle = v_valve_output(p_chamber, t_chamber, valve_states) thrust, imp_sp, mass_flow = v_nozzle(p_nozzle, t_nozzle) e_sun_eci = [sun_vector(t_i) for t_i in t] e_sun_b = [ quat_obj_i.rotate(e_sun_eci_i) for quat_obj_i, e_sun_eci_i in zip(quat_obj, e_sun_eci) ] o1_eci = [ a_io(r_vector_i, v_vector_i)[:, 0] for r_vector_i, v_vector_i in zip(r_vector, v_vector) ] o1_b = [ quat_obj_i.rotate(o1_eci_i) for quat_obj_i, o1_eci_i in zip(quat_obj, o1_eci) ] q_cmd = [ command_quaternion(t_i, y_T_i, sat_states_i, strategy) for t_i, y_T_i, sat_states_i in zip(t, y.T, sat_states) ] q_err = [ error_quaternion(quat_obj_i, q_cmd_i) for quat_obj_i, q_cmd_i in zip(quat_obj, q_cmd) ] torque_cmd = [ -1 * np.dot(K_D[sat_states_i], omega_bi_b_i) - np.dot(K_P[sat_states_i], quat_err_i.vector) for sat_states_i, omega_bi_b_i, quat_err_i in zip(sat_states, omega_bi_b, q_err) ] rw_acc = [ wheels_acceleration(torque_cmd_i, wheels_rate_i) for torque_cmd_i, wheels_rate_i in zip(torque_cmd, wheels_rate) ] res = [ p_chamber, t_chamber, m_steam, m_liq, thrust, imp_sp, mass_flow, e_sun_b, o1_b, q_cmd, q_err, torque_cmd, rw_acc, ] return res # %% if __name__ == "__main__": sat1_sol = [] sat2_sol = [] sim_start_time = datetime.now() print(f"Simulation started at {sim_start_time}") if ALPHA_GOAL is not None: # use only half of the available fuel if DEBUG_PRINT: print(f"\n\n{'=' * 40}\nIntermediate orbits transfer\n\n") ( sat_solutions, free_flight_single_sol, sat_y_last_points, ) = two_satellites_maneuvering( 0, MANEUVERING_MAX_DURATION, 0.5 * (M_FUEL_0 + FINAL_FUEL_MASS), [None, None], ["increase periapsis", "decrease apoapsis"], ) t0 = free_flight_single_sol.t[-1] ( free_flight_phasing_sol, sat_y_last_points, ) = free_flight_phasing(t0, sat_y_last_points, ALPHA_GOAL) t0 = free_flight_phasing_sol.t[-1] if DEBUG_PRINT: print(f"\n\n{'=' * 40}\nReturn back to initial orbit\n\n") func_output = two_satellites_maneuvering( t0, t0 + MANEUVERING_MAX_DURATION, FINAL_FUEL_MASS, sat_y_last_points, ["decrease apoapsis", "increase periapsis"], ) sat_solutions = [sat_solutions, func_output[0]] free_flight_single_sol = [free_flight_single_sol, func_output[1]] sat_y_last_points = func_output[2] r_last_points = [y[2:5] for y in sat_y_last_points] alpha_final = math.acos(phasing_angle(*r_last_points)) print(f"\n\nFinal angle: {math.degrees(alpha_final)}, degrees") else: # use all available fuel sat_solutions = [ simulation_launch(0, MANEUVERING_MAX_DURATION, FINAL_FUEL_MASS) ] sim_end_time = datetime.now() print(f"Whole simulation finished at {sim_end_time}") other_parameters = [ [define_other_parameters(sat_sol) for sat_sol in phase] for phase in sat_solutions ] if ALPHA_GOAL is not None: results = [ sat_solutions, other_parameters, free_flight_phasing_sol, free_flight_single_sol, ] else: results = [sat_solutions, other_parameters] Path(RESULTS_FOLDER).mkdir(parents=True, exist_ok=True) export_date = datetime.now().strftime("%Y-%m-%d-%H:%M:%S") filename = f"{RESULTS_FOLDER}/{MISSION_PREFIX}-res-{export_date}" with open(filename, "wb") as res_output_file: pickle.dump(results, res_output_file) # %%