衛星衝突回避の理論 — 接近解析と衝突確率の計算

import numpy as np
from scipy.integrate import solve_ivp
from scipy.optimize import minimize_scalar
import matplotlib.pyplot as plt

# --- 定数 ---
mu = 3.986004418e14     # 地球の重力パラメータ [m^3/s^2]
R_E = 6378.137e3         # 地球半径 [m]


def kep_to_cart(a, e, i, RAAN, omega, nu):
    """ケプラー要素 → 位置・速度"""
    p = a * (1 - e**2)
    r_mag = p / (1 + e * np.cos(nu))

    r_pqw = r_mag * np.array([np.cos(nu), np.sin(nu), 0.0])
    v_pqw = np.sqrt(mu / p) * np.array([-np.sin(nu), e + np.cos(nu), 0.0])

    cO, sO = np.cos(RAAN), np.sin(RAAN)
    ci, si = np.cos(i), np.sin(i)
    cw, sw = np.cos(omega), np.sin(omega)

    R_mat = np.array([
        [cO*cw - sO*sw*ci, -cO*sw - sO*cw*ci, sO*si],
        [sO*cw + cO*sw*ci, -sO*sw + cO*cw*ci, -cO*si],
        [sw*si, cw*si, ci]
    ])

    return R_mat @ r_pqw, R_mat @ v_pqw


def two_body_eom(t, state):
    """二体問題の運動方程式"""
    r_vec = state[0:3]
    v_vec = state[3:6]
    r = np.linalg.norm(r_vec)
    a = -mu / r**3 * r_vec
    return np.concatenate([v_vec, a])

# --- 衛星1: ISS風の軌道 ---
a1 = R_E + 420e3
e1 = 0.0005
i1 = np.radians(51.6)
RAAN1 = np.radians(10.0)
omega1 = np.radians(0.0)
nu1 = np.radians(0.0)

# --- 衛星2（デブリ）: わずかに異なる軌道 ---
a2 = R_E + 418e3
e2 = 0.001
i2 = np.radians(97.0)      # 太陽同期軌道に近い傾斜角（交差角が大きい）
RAAN2 = np.radians(12.0)
omega2 = np.radians(5.0)
nu2 = np.radians(180.0)

# 初期状態
r1_0, v1_0 = kep_to_cart(a1, e1, i1, RAAN1, omega1, nu1)
r2_0, v2_0 = kep_to_cart(a2, e2, i2, RAAN2, omega2, nu2)

state1_0 = np.concatenate([r1_0, v1_0])
state2_0 = np.concatenate([r2_0, v2_0])

# --- 軌道を伝播して接近を検出 ---
T_sim = 2 * 86400   # 2日間
t_eval_coarse = np.linspace(0, T_sim, 20000)

sol1 = solve_ivp(two_body_eom, (0, T_sim), state1_0,
                 method='DOP853', t_eval=t_eval_coarse,
                 rtol=1e-12, atol=1e-14)
sol2 = solve_ivp(two_body_eom, (0, T_sim), state2_0,
                 method='DOP853', t_eval=t_eval_coarse,
                 rtol=1e-12, atol=1e-14)

# 各時刻での距離を計算
distances = np.zeros(len(t_eval_coarse))
for k in range(len(t_eval_coarse)):
    dr = sol2.y[0:3, k] - sol1.y[0:3, k]
    distances[k] = np.linalg.norm(dr)

# 接近イベントの検出（距離が100 km以内）
threshold = 100e3  # 100 km
close_mask = distances < threshold
close_indices = np.where(close_mask)[0]

if len(close_indices) > 0:
    # 最も近い接近を探す
    min_idx = np.argmin(distances)
    print(f"最接近距離（粗い探索）: {distances[min_idx]/1e3:.2f} km")
    print(f"最接近時刻（粗い探索）: {t_eval_coarse[min_idx]/3600:.2f} hours")
else:
    print("接近イベントなし（閾値内に入りませんでした）")
    # 全体の最小距離を表示
    min_idx = np.argmin(distances)
    print(f"全体の最小距離: {distances[min_idx]/1e3:.2f} km")

# --- TCA付近の精密計算 ---
def distance_at_time(t_sec):
    """時刻tでの2物体間の距離"""
    sol1_t = solve_ivp(two_body_eom, (0, t_sec), state1_0,
                       method='DOP853', t_eval=[t_sec],
                       rtol=1e-13, atol=1e-15)
    sol2_t = solve_ivp(two_body_eom, (0, t_sec), state2_0,
                       method='DOP853', t_eval=[t_sec],
                       rtol=1e-13, atol=1e-15)
    dr = sol2_t.y[0:3, 0] - sol1_t.y[0:3, 0]
    return np.linalg.norm(dr)

# 粗い探索で見つかった最接近時刻付近で精密探索
t_rough = t_eval_coarse[min_idx]
t_search_min = max(0, t_rough - 300)
t_search_max = min(T_sim, t_rough + 300)

result = minimize_scalar(distance_at_time,
                         bounds=(t_search_min, t_search_max),
                         method='bounded',
                         options={'xatol': 0.001})

t_tca = result.x
d_min = result.fun

print(f"\n=== TCA精密計算結果 ===")
print(f"TCA時刻: {t_tca:.3f} s ({t_tca/3600:.4f} hours)")
print(f"最接近距離: {d_min:.2f} m ({d_min/1e3:.4f} km)")

# --- TCAでの状態ベクトルを取得 ---
sol1_tca = solve_ivp(two_body_eom, (0, t_tca), state1_0,
                     method='DOP853', t_eval=[t_tca],
                     rtol=1e-13, atol=1e-15)
sol2_tca = solve_ivp(two_body_eom, (0, t_tca), state2_0,
                     method='DOP853', t_eval=[t_tca],
                     rtol=1e-13, atol=1e-15)

r1_tca = sol1_tca.y[0:3, 0]
v1_tca = sol1_tca.y[3:6, 0]
r2_tca = sol2_tca.y[0:3, 0]
v2_tca = sol2_tca.y[3:6, 0]

# 相対ベクトル
rho_tca = r2_tca - r1_tca
rho_dot = v2_tca - v1_tca

print(f"\n相対位置ベクトル: {rho_tca} m")
print(f"相対速度: {np.linalg.norm(rho_dot)/1e3:.2f} km/s")
print(f"最接近距離: {np.linalg.norm(rho_tca):.2f} m")

def compute_collision_probability(rho_tca, rho_dot, P1_pos, P2_pos, R_HBR):
    """
    2次元Alfriend法による衝突確率の計算

    Parameters:
        rho_tca: 最接近時の相対位置ベクトル [m] (3,)
        rho_dot: 最接近時の相対速度ベクトル [m/s] (3,)
        P1_pos: 物体1の位置共分散行列 [m^2] (3,3)
        P2_pos: 物体2の位置共分散行列 [m^2] (3,3)
        R_HBR: 結合ハードボディ半径 [m]

    Returns:
        P_c: 衝突確率
    """
    # 結合共分散行列
    P_c_3d = P1_pos + P2_pos

    # 接近平面の基底ベクトル（相対速度に垂直な面）
    v_hat = rho_dot / np.linalg.norm(rho_dot)

    # 接近平面の基底を構築
    # v_hat に垂直な2つのベクトルを見つける
    if abs(v_hat[0]) < 0.9:
        e1 = np.cross(v_hat, [1, 0, 0])
    else:
        e1 = np.cross(v_hat, [0, 1, 0])
    e1 = e1 / np.linalg.norm(e1)
    e2 = np.cross(v_hat, e1)
    e2 = e2 / np.linalg.norm(e2)

    # 投影行列
    T = np.array([e1, e2])  # (2, 3)

    # 2次元に投影
    P_2d = T @ P_c_3d @ T.T  # (2, 2)
    miss_2d = T @ rho_tca     # (2,)

    # 数値積分で衝突確率を計算
    # ガウス分布の2次元積分（円形ハードボディ領域）
    P_2d_inv = np.linalg.inv(P_2d)
    det_P = np.linalg.det(P_2d)

    # 極座標積分
    n_r = 200
    n_theta = 200
    r_vals = np.linspace(0, R_HBR, n_r)
    theta_vals = np.linspace(0, 2 * np.pi, n_theta)

    P_collision = 0.0
    dr = r_vals[1] - r_vals[0]
    dtheta = theta_vals[1] - theta_vals[0]

    for r in r_vals:
        for theta in theta_vals:
            u = np.array([r * np.cos(theta), r * np.sin(theta)]) - miss_2d
            exponent = -0.5 * u @ P_2d_inv @ u
            P_collision += r * np.exp(exponent) * dr * dtheta

    P_collision /= (2 * np.pi * np.sqrt(det_P))

    return P_collision, miss_2d, P_2d


# --- 共分散行列の設定 ---
# 衛星1の位置共分散（追跡精度が良い）
sigma1_along = 200.0    # 接線方向 [m]
sigma1_cross = 30.0     # 交差方向 [m]
sigma1_radial = 10.0    # 動径方向 [m]
P1_pos = np.diag([sigma1_along**2, sigma1_cross**2, sigma1_radial**2])

# 衛星2（デブリ）の位置共分散（追跡精度が悪い）
sigma2_along = 1000.0
sigma2_cross = 100.0
sigma2_radial = 50.0
P2_pos = np.diag([sigma2_along**2, sigma2_cross**2, sigma2_radial**2])

# 結合ハードボディ半径
R_HBR = 10.0  # [m]（大型衛星 + デブリ）

# 衝突確率の計算
P_c, miss_2d, P_2d = compute_collision_probability(
    rho_tca, rho_dot, P1_pos, P2_pos, R_HBR)

print(f"\n=== 衝突確率計算結果 ===")
print(f"衝突確率: {P_c:.6e}")
print(f"接近平面上のミス距離: ({miss_2d[0]:.1f}, {miss_2d[1]:.1f}) m")
print(f"ミス距離の大きさ: {np.linalg.norm(miss_2d):.1f} m")

# リスク判定
if P_c > 1e-4:
    print("→ レッドアラート: 回避マヌーバを推奨")
elif P_c > 1e-5:
    print("→ イエローアラート: 状況を注視、マヌーバ準備")
else:
    print("→ 許容範囲内: アクション不要")

# --- 接近平面上の可視化 ---
fig, ax = plt.subplots(figsize=(8, 8))

# 誤差楕円体の可視化
eigenvalues, eigenvectors = np.linalg.eigh(P_2d)
angle = np.degrees(np.arctan2(eigenvectors[1, 0], eigenvectors[0, 0]))

for n_sigma, alpha in [(1, 0.3), (2, 0.2), (3, 0.1)]:
    ellipse = plt.matplotlib.patches.Ellipse(
        (0, 0),
        2 * n_sigma * np.sqrt(eigenvalues[0]),
        2 * n_sigma * np.sqrt(eigenvalues[1]),
        angle=angle,
        fill=True, facecolor='blue', alpha=alpha,
        edgecolor='blue', linewidth=1,
        label=f'{n_sigma}σ' if n_sigma == 1 else None)
    ax.add_patch(ellipse)

# ハードボディ円
circle = plt.Circle(miss_2d, R_HBR, fill=False, color='red',
                     linewidth=2, label=f'Hard-body (R={R_HBR}m)')
ax.add_patch(circle)

# ミス距離ベクトル
ax.plot([0, miss_2d[0]], [0, miss_2d[1]], 'r--', linewidth=1.5)
ax.plot(miss_2d[0], miss_2d[1], 'r*', markersize=15, label='Miss point')
ax.plot(0, 0, 'b+', markersize=15, markeredgewidth=2, label='Origin (predicted)')

# 軸の設定
max_range = max(3 * np.sqrt(eigenvalues.max()),
                np.linalg.norm(miss_2d) + R_HBR) * 1.2
ax.set_xlim(-max_range, max_range)
ax.set_ylim(-max_range, max_range)
ax.set_aspect('equal')
ax.set_xlabel('Encounter plane ξ [m]')
ax.set_ylabel('Encounter plane η [m]')
ax.set_title(f'接近平面上の誤差楕円体と衝突領域\n'
             f'Miss distance = {np.linalg.norm(miss_2d):.1f} m, '
             f'P_c = {P_c:.2e}')
ax.legend(loc='upper right')
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('conjunction_assessment.png', dpi=150, bbox_inches='tight')
plt.show()

# --- ミス距離と衝突確率の関係 ---
miss_distances = np.linspace(10, 5000, 50)
Pc_values = []

for d_miss in miss_distances:
    # ミス距離をスケーリング
    rho_scaled = rho_tca * (d_miss / np.linalg.norm(rho_tca))
    Pc_val, _, _ = compute_collision_probability(
        rho_scaled, rho_dot, P1_pos, P2_pos, R_HBR)
    Pc_values.append(Pc_val)

fig, ax = plt.subplots(figsize=(10, 6))
ax.semilogy(miss_distances, Pc_values, 'b-', linewidth=2)
ax.axhline(y=1e-4, color='red', linestyle='--', alpha=0.7, label='Red threshold (1e-4)')
ax.axhline(y=1e-5, color='orange', linestyle='--', alpha=0.7, label='Yellow threshold (1e-5)')
ax.set_xlabel('Miss Distance [m]')
ax.set_ylabel('Collision Probability')
ax.set_title('ミス距離と衝突確率の関係')
ax.legend()
ax.grid(True, alpha=0.3, which='both')
plt.tight_layout()
plt.savefig('pc_vs_miss_distance.png', dpi=150, bbox_inches='tight')
plt.show()