CW.zip_CW方程_shake6s2_求解cw方程_相对导航控制_轨道规划

import numpy as np import matplotlib as mpl from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt from math import * import time dt = np.array([1,1,1]) T = 10000 T_mat = list(range(T+1)) u = 3.986012e14 r = 6378137 a = 7e6 n = sqrt(u/(a+r)**3) location = np.array([10000,20000,-10000]) #相对位置 vel = np.array([1,-2,3]) #相对速度 accel = np.array([]) #加速度 accel_mat = [[],[],[]] vel_mat = [[1],[-2],[3]] location_mat = [[10000],[20000],[-10000]] for i in range(0,T) : accel = np.array([2*n*vel[1]+3*n**2*location[0], -2*n*vel[0], -n**2*location[2]]) vel = np.array(vel+accel*dt) location = np.array(location+vel*dt) accel_mat[0].append(accel[0]) accel_mat[1].append(accel[1]) accel_mat[2].append(accel[2]) vel_mat[0].append(vel[0]) vel_mat[1].append(vel[1]) vel_mat[2].append(vel[2]) location_mat[0].append(location[0]) location_mat[1].append(location[1]) location_mat[2].append(location[2]) plt.subplot(2,1,1) plt.plot(T_mat[0:-1],accel_mat[0]) plt.plot(T_mat[0:-1],accel_mat[1]) plt.plot(T_mat[0:-1],accel_mat[2]) plt.subplot(2,1,2) plt.plot(T_mat,vel_mat[0]) plt.plot(T_mat,vel_mat[1]) plt.plot(T_mat,vel_mat[2]) mpl.rcParams['legend.fontsize'] = 10 fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(location_mat[0], location_mat[1], location_mat[2], label='parametric curve') ax.legend() plt.show()