惑星探査ローバーの移動機構と走行力学 — 他の天体を走るロボット

import numpy as np
import matplotlib.pyplot as plt

# --- 車輪パラメータ（Curiosity相当）---
r_wheel = 0.25       # 車輪半径 [m]
b_wheel = 0.40       # 車輪幅 [m]
mass_rover = 899      # ローバー質量 [kg]
n_wheels = 6          # 車輪数

# --- 土壌パラメータ（火星レゴリス相当） ---
c_soil = 0.5e3        # 粘着力 [Pa]（砂地、ほぼ0に近い）
phi_soil = np.radians(35)  # 内部摩擦角 [rad]
K_shear = 0.02        # せん断変形係数 [m]

# --- 各天体の重力加速度 ---
g_earth = 9.81
g_mars = 3.72
g_moon = 1.62

def traction_force(slip, W, b, l, c, phi, K):
    """Janosi-Hanamoroモデルによる牽引力"""
    sigma = W / (b * l)  # 平均接地圧
    tau_max = c + sigma * np.tan(phi)
    # スリップ率0で0除算を避ける
    s_safe = np.where(slip > 1e-10, slip, 1e-10)
    F_D = b * l * tau_max * (1 - (K / (s_safe * l)) * (1 - np.exp(-s_safe * l / K)))
    return F_D

# スリップ率の範囲
slip_range = np.linspace(0.001, 0.99, 500)

fig, axes = plt.subplots(1, 2, figsize=(14, 5))

# --- 左パネル: 各天体での牽引力 ---
for g_val, label, color in [(g_earth, 'Earth (g=9.81)', '#2196F3'),
                              (g_mars, 'Mars (g=3.72)', '#FF5722'),
                              (g_moon, 'Moon (g=1.62)', '#9E9E9E')]:
    W_per_wheel = mass_rover * g_val / n_wheels
    z0 = 0.03  # 仮定: 沈下深さ 3cm
    l_contact = np.sqrt(2 * r_wheel * z0)
    F_D = traction_force(slip_range, W_per_wheel, b_wheel, l_contact,
                         c_soil, phi_soil, K_shear)
    axes[0].plot(slip_range * 100, F_D, label=label, color=color, linewidth=2)

axes[0].set_xlabel('Slip ratio [%]', fontsize=12)
axes[0].set_ylabel('Traction force per wheel [N]', fontsize=12)
axes[0].set_title('Traction Force vs Slip Ratio (Janosi-Hanamoto)', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3)
axes[0].set_xlim(0, 100)

# --- 右パネル: 正味駆動力（牽引力 - 圧密抵抗）---
# ベッカーモデル: 圧密抵抗
kc = 1.4e3    # [N/m^(n+1)]
kphi = 820e3  # [N/m^(n+2)]
n_bekker = 1.0

def compaction_resistance(z0, b, kc, kphi, n):
    """ベッカーモデルによる圧密抵抗"""
    return b * (kc / b + kphi) * z0**(n + 1) / (n + 1)

# 火星環境でのスリップ率 vs 正味駆動力
g_val = g_mars
W_per_wheel = mass_rover * g_val / n_wheels
z0_values = [0.01, 0.03, 0.05, 0.08]  # 沈下深さ [m]

for z0 in z0_values:
    l_contact = np.sqrt(2 * r_wheel * z0)
    F_D = traction_force(slip_range, W_per_wheel, b_wheel, l_contact,
                         c_soil, phi_soil, K_shear)
    F_R = compaction_resistance(z0, b_wheel, kc, kphi, n_bekker)
    F_net = F_D - F_R
    axes[1].plot(slip_range * 100, F_net,
                 label=f'Sinkage z₀ = {z0*100:.0f} cm', linewidth=2)

axes[1].axhline(y=0, color='red', linestyle='--', alpha=0.5, label='F_net = 0 (stuck)')
axes[1].set_xlabel('Slip ratio [%]', fontsize=12)
axes[1].set_ylabel('Net driving force per wheel [N]', fontsize=12)
axes[1].set_title('Net Driving Force on Mars', fontsize=13)
axes[1].legend(fontsize=10)
axes[1].grid(True, alpha=0.3)
axes[1].set_xlim(0, 100)

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

import numpy as np
import matplotlib.pyplot as plt

# --- パラメータ ---
mass_rover = 899     # [kg] Curiosity相当
n_wheels = 6
g_mars = 3.72        # [m/s^2]
b_wheel = 0.40       # [m]
W_per_wheel = mass_rover * g_mars / n_wheels

# ベッカー土壌パラメータ（火星レゴリス）
kc = 1.4e3
kphi = 820e3
n_bekker = 1.0

# 車輪直径の範囲
diameters = np.linspace(0.15, 0.80, 200)  # [m]

# 沈下深さの計算（荷重と土壌支持力の平衡から近似的に求める）
def compute_sinkage(W, b, r, kc, kphi, n):
    """荷重平衡から沈下深さを推定（反復法）"""
    z0 = 0.01  # 初期推定
    for _ in range(100):
        l = np.sqrt(2 * r * z0) if z0 > 0 else 0.01
        # 接地面での平均圧力 = 荷重 / 接地面積
        p_mean = W / (b * l)
        # ベッカーモデルでの圧力-沈下関係から沈下を更新
        z0_new = (p_mean / (kc / b + kphi))**(1.0 / n)
        if abs(z0_new - z0) < 1e-6:
            break
        z0 = 0.5 * z0 + 0.5 * z0_new  # 緩和法で安定化
    return z0

sinkages = []
for d in diameters:
    r = d / 2
    z0 = compute_sinkage(W_per_wheel, b_wheel, r, kc, kphi, n_bekker)
    sinkages.append(z0 * 100)  # cm変換

# プロット
fig, ax = plt.subplots(figsize=(8, 5))
ax.plot(diameters * 100, sinkages, 'b-', linewidth=2.5)

# 実際のローバーの車輪直径をマーク
rovers = {
    'Sojourner': 13,
    'Spirit/Opportunity': 26,
    'Curiosity': 50,
    'Perseverance': 52.5,
}
for name, diam in rovers.items():
    r = diam / 200  # mに変換
    z0 = compute_sinkage(W_per_wheel, b_wheel, r, kc, kphi, n_bekker)
    ax.plot(diam, z0 * 100, 'o', markersize=10, zorder=5)
    ax.annotate(name, (diam, z0 * 100), textcoords="offset points",
                xytext=(10, 5), fontsize=10)

ax.set_xlabel('Wheel diameter [cm]', fontsize=12)
ax.set_ylabel('Sinkage depth [cm]', fontsize=12)
ax.set_title('Wheel Diameter vs Sinkage Depth on Mars', fontsize=13)
ax.grid(True, alpha=0.3)
ax.set_xlim(10, 85)
plt.tight_layout()
plt.savefig('rover_sinkage.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle

class RockerBogie:
    """ロッカーボギー機構の2Dキネマティクスモデル"""

    def __init__(self, L_bogie_front=0.40, L_bogie_rear=0.40,
                 L_rocker_front=0.55, L_rocker_rear=0.65,
                 wheel_radius=0.25, wheel_base_y=0.80):
        """
        Parameters
        ----------
        L_bogie_front : float - ボギーピボットから前輪までの距離 [m]
        L_bogie_rear  : float - ボギーピボットから中輪までの距離 [m]
        L_rocker_front: float - ロッカーピボットからボギーピボットまでの距離 [m]
        L_rocker_rear : float - ロッカーピボットから後輪までの距離 [m]
        wheel_radius  : float - 車輪半径 [m]
        wheel_base_y  : float - 左右車輪間距離（片側）[m]
        """
        self.Lbf = L_bogie_front
        self.Lbr = L_bogie_rear
        self.Lrf = L_rocker_front
        self.Lrr = L_rocker_rear
        self.r = wheel_radius
        self.y_offset = wheel_base_y

    def compute_kinematics(self, terrain_func, x_body):
        """
        車体中心のx座標を指定して、各車輪位置と車体姿勢を計算する

        Parameters
        ----------
        terrain_func : callable - x -> z の地形関数
        x_body       : float   - 車体中心のx座標

        Returns
        -------
        dict : 各部の位置と角度
        """
        # 車輪のx座標（車体中心からの相対位置）
        x_front = x_body + self.Lrf + self.Lbf  # 前輪
        x_mid = x_body + self.Lrf - self.Lbr    # 中輪
        x_rear = x_body - self.Lrr               # 後輪

        # 各車輪の接地点高さ（地形 + 車輪半径）
        z_front = terrain_func(x_front) + self.r
        z_mid = terrain_func(x_mid) + self.r
        z_rear = terrain_func(x_rear) + self.r

        # ボギー角度
        dx_bogie = x_mid - x_front
        dz_bogie = z_mid - z_front
        theta_bogie = np.arctan2(dz_bogie, abs(dx_bogie))

        # ボギーピボットの位置
        bogie_pivot_x = x_front + self.Lbf * np.cos(theta_bogie)
        bogie_pivot_z = z_front + self.Lbf * np.sin(theta_bogie)

        # ロッカー角度
        dx_rocker = x_rear - bogie_pivot_x
        dz_rocker = z_rear - bogie_pivot_z
        theta_rocker = np.arctan2(dz_rocker, abs(dx_rocker))

        # 車体ピッチ角（ディファレンシャルで半分に）
        theta_body = theta_rocker  # 2D片側モデルなので近似

        # 車体中心の高さ
        z_body = (bogie_pivot_z + z_rear) / 2 + 0.30  # 車体オフセット

        return {
            'wheels': [(x_front, z_front), (x_mid, z_mid), (x_rear, z_rear)],
            'bogie_pivot': (bogie_pivot_x, bogie_pivot_z),
            'theta_bogie': theta_bogie,
            'theta_rocker': theta_rocker,
            'theta_body': np.degrees(theta_body),
            'body_center': (x_body, z_body),
        }


def terrain_with_obstacles(x):
    """岩場を模擬した地形関数"""
    z = np.zeros_like(np.atleast_1d(x).astype(float))
    x = np.atleast_1d(x).astype(float)
    # 障害物1: 高さ15cmの岩（x=2.0付近）
    mask1 = (x > 1.8) & (x < 2.2)
    z[mask1] = 0.15 * np.cos((x[mask1] - 2.0) / 0.2 * np.pi / 2)
    # 障害物2: 高さ20cmの岩（x=4.0付近）
    mask2 = (x > 3.7) & (x < 4.3)
    z[mask2] = 0.20 * np.cos((x[mask2] - 4.0) / 0.3 * np.pi / 2)
    # なだらかな起伏
    z += 0.05 * np.sin(x * 0.8)
    return z if len(z) > 1 else z[0]


# シミュレーション実行
rb = RockerBogie()
x_positions = np.linspace(0.5, 6.0, 300)

body_pitch_angles = []
body_heights = []

for x_b in x_positions:
    result = rb.compute_kinematics(terrain_with_obstacles, x_b)
    body_pitch_angles.append(result['theta_body'])
    body_heights.append(result['body_center'][1])

# --- 可視化 ---
fig, axes = plt.subplots(2, 1, figsize=(12, 8), gridspec_kw={'height_ratios': [2, 1]})

# 上段: 地形と車体高さ
x_terrain = np.linspace(0, 7, 1000)
z_terrain = terrain_with_obstacles(x_terrain)

axes[0].fill_between(x_terrain, z_terrain, -0.1, color='#8B7355', alpha=0.3, label='Terrain')
axes[0].plot(x_terrain, z_terrain, color='#8B7355', linewidth=2)
axes[0].plot(x_positions, body_heights, 'b-', linewidth=2, label='Body height')
axes[0].set_ylabel('Height [m]', fontsize=12)
axes[0].set_title('Rocker-Bogie Traversal over Rocky Terrain', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3)
axes[0].set_xlim(0, 6.5)

# 特定位置でのローバー姿勢を描画
for x_snapshot in [1.0, 2.0, 3.0, 4.0, 5.0]:
    result = rb.compute_kinematics(terrain_with_obstacles, x_snapshot)
    wheels = result['wheels']
    bp = result['bogie_pivot']

    for wx, wz in wheels:
        circle = Circle((wx, wz), rb.r, fill=False, edgecolor='red', linewidth=1.5)
        axes[0].add_patch(circle)

    # ボギーリンク（前輪-中輪）
    axes[0].plot([wheels[0][0], bp[0]], [wheels[0][1], bp[1]], 'k-', linewidth=1.5)
    axes[0].plot([wheels[1][0], bp[0]], [wheels[1][1], bp[1]], 'k-', linewidth=1.5)
    # ロッカーリンク（ボギーピボット-後輪）
    axes[0].plot([bp[0], wheels[2][0]], [bp[1], wheels[2][1]], 'k-', linewidth=1.5)

axes[0].set_ylim(-0.1, 1.2)

# 下段: ピッチ角の変化
axes[1].plot(x_positions, body_pitch_angles, 'r-', linewidth=2)
axes[1].set_xlabel('Body x-position [m]', fontsize=12)
axes[1].set_ylabel('Body pitch [deg]', fontsize=12)
axes[1].set_title('Body Pitch Angle during Traversal', fontsize=13)
axes[1].grid(True, alpha=0.3)
axes[1].set_xlim(0, 6.5)
axes[1].axhline(y=0, color='gray', linestyle='--', alpha=0.5)

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

import numpy as np
import matplotlib.pyplot as plt

# --- パラメータ ---
g_mars = 3.72  # [m/s^2]
mass = 200     # [kg] 中型ローバー想定

# 各方式の簡易モデル
# 輸送コスト c_T = P / (m * g * v) のモデル

def cost_of_transport_wheel(slope_deg, mu_roll=0.03, eta_motor=0.8):
    """車輪方式の輸送コスト"""
    slope_rad = np.radians(slope_deg)
    # 走行抵抗: 転がり抵抗 + 重力成分
    c_T = (mu_roll * np.cos(slope_rad) + np.sin(slope_rad)) / eta_motor
    return c_T

def cost_of_transport_track(slope_deg, mu_roll=0.08, eta_motor=0.7):
    """クローラ方式の輸送コスト"""
    slope_rad = np.radians(slope_deg)
    c_T = (mu_roll * np.cos(slope_rad) + np.sin(slope_rad)) / eta_motor
    return c_T

def cost_of_transport_leg(slope_deg, c0=0.5, eta_motor=0.6):
    """脚方式の輸送コスト（歩行のオーバーヘッドが大きい）"""
    slope_rad = np.radians(slope_deg)
    # 基本コスト（脚の上げ下げ） + 勾配成分
    c_T = (c0 + np.sin(slope_rad)) / eta_motor
    return c_T

def cost_of_transport_hybrid(slope_deg, mu_roll=0.04, c0_leg=0.2, eta_motor=0.75,
                              transition_slope=15):
    """ハイブリッド方式（低勾配は車輪、高勾配は脚寄り）"""
    slope_rad = np.radians(slope_deg)
    # 勾配に応じて車輪モードと脚モードをブレンド
    alpha = np.clip((slope_deg - transition_slope) / 10, 0, 1)
    c_wheel = (mu_roll * np.cos(slope_rad) + np.sin(slope_rad))
    c_leg = (c0_leg + np.sin(slope_rad))
    c_T = ((1 - alpha) * c_wheel + alpha * c_leg) / eta_motor
    return c_T

slopes = np.linspace(0, 35, 200)

fig, ax = plt.subplots(figsize=(10, 6))

methods = [
    ('Wheel (Rocker-Bogie)', cost_of_transport_wheel, '#2196F3', '-'),
    ('Tracked (Crawler)', cost_of_transport_track, '#FF9800', '--'),
    ('Legged (Hexapod)', cost_of_transport_leg, '#F44336', '-.'),
    ('Hybrid (Wheel-Leg)', cost_of_transport_hybrid, '#4CAF50', ':'),
]

for name, func, color, ls in methods:
    c_T = func(slopes)
    ax.plot(slopes, c_T, label=name, color=color, linewidth=2.5, linestyle=ls)

ax.set_xlabel('Terrain slope [deg]', fontsize=12)
ax.set_ylabel('Cost of Transport $c_T$ [-]', fontsize=12)
ax.set_title('Cost of Transport vs Terrain Slope (Mars Gravity)', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
ax.set_xlim(0, 35)
ax.set_ylim(0, 1.5)

# 典型的な火星地形の傾斜範囲を示す
ax.axvspan(0, 10, alpha=0.08, color='green', label='_Typical flat')
ax.axvspan(15, 25, alpha=0.08, color='orange', label='_Hilly terrain')
ax.text(5, 1.4, 'Flat terrain', fontsize=10, ha='center', color='green')
ax.text(20, 1.4, 'Hilly terrain', fontsize=10, ha='center', color='orange')

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

import numpy as np
import matplotlib.pyplot as plt

# --- 各ローバーの仕様 ---
rovers = {
    'Sojourner': {
        'mass': 10.6, 'n_wheels': 6, 'r_wheel': 0.065,
        'b_wheel': 0.08, 'color': '#9C27B0'
    },
    'Spirit/Opportunity': {
        'mass': 185, 'n_wheels': 6, 'r_wheel': 0.13,
        'b_wheel': 0.16, 'color': '#2196F3'
    },
    'Curiosity': {
        'mass': 899, 'n_wheels': 6, 'r_wheel': 0.25,
        'b_wheel': 0.40, 'color': '#FF5722'
    },
    'Perseverance': {
        'mass': 1025, 'n_wheels': 6, 'r_wheel': 0.2625,
        'b_wheel': 0.40, 'color': '#4CAF50'
    },
    'Yutu-2': {
        'mass': 140, 'n_wheels': 6, 'r_wheel': 0.15,
        'b_wheel': 0.20, 'color': '#FF9800'
    },
}

# 火星レゴリス土壌パラメータ
c_soil = 0.5e3       # [Pa]
phi_soil = np.radians(35)
K_shear = 0.02       # [m]
kc = 1.4e3
kphi = 820e3
n_bekker = 1.0

g_mars = 3.72  # [m/s^2]（玉兔2号は月面だが比較のため火星で統一）

def compute_max_slope(mass, n_wheels, r_wheel, b_wheel, g,
                      c, phi, K, kc, kphi, n_bek, slip=0.2):
    """与えられたスリップ率での最大登坂角度を二分法で求める"""
    W_total = mass * g

    for slope_deg in np.arange(0, 45, 0.5):
        slope_rad = np.radians(slope_deg)
        # 斜面上の荷重配分（簡略化: 均等配分）
        W_normal = W_total * np.cos(slope_rad) / n_wheels  # 法線方向荷重/1輪
        F_gravity = W_total * np.sin(slope_rad)  # 重力の斜面方向成分

        # 沈下深さの推定
        z0 = 0.01
        for _ in range(50):
            l = np.sqrt(2 * r_wheel * z0) if z0 > 0 else 0.01
            p_mean = W_normal / (b_wheel * l)
            z0_new = (p_mean / (kc / b_wheel + kphi))**(1.0 / n_bek)
            if abs(z0_new - z0) < 1e-7:
                break
            z0 = 0.5 * z0 + 0.5 * z0_new

        l = np.sqrt(2 * r_wheel * z0)

        # 牽引力（全輪合計）
        sigma = W_normal / (b_wheel * l)
        tau_max = c + sigma * np.tan(phi)
        s_safe = max(slip, 1e-10)
        F_D_per_wheel = b_wheel * l * tau_max * (
            1 - (K / (s_safe * l)) * (1 - np.exp(-s_safe * l / K))
        )
        F_D_total = F_D_per_wheel * n_wheels

        # 圧密抵抗（全輪合計）
        F_R_per_wheel = b_wheel * (kc / b_wheel + kphi) * z0**(n_bek + 1) / (n_bek + 1)
        F_R_total = F_R_per_wheel * n_wheels

        # 正味駆動力 = 牽引力 - 圧密抵抗 - 重力斜面成分
        F_net = F_D_total - F_R_total - F_gravity
        if F_net < 0:
            return slope_deg
    return 45.0

# --- スリップ率を変えた最大登坂角度 ---
slip_range = np.linspace(0.05, 0.60, 100)

fig, axes = plt.subplots(1, 2, figsize=(14, 5))

# 左パネル: スリップ率 vs 最大登坂角度
for name, spec in rovers.items():
    max_slopes = []
    for s in slip_range:
        ms = compute_max_slope(
            spec['mass'], spec['n_wheels'], spec['r_wheel'], spec['b_wheel'],
            g_mars, c_soil, phi_soil, K_shear, kc, kphi, n_bekker, slip=s
        )
        max_slopes.append(ms)
    axes[0].plot(slip_range * 100, max_slopes, label=name,
                 color=spec['color'], linewidth=2.5)

axes[0].set_xlabel('Slip ratio [%]', fontsize=12)
axes[0].set_ylabel('Max climbable slope [deg]', fontsize=12)
axes[0].set_title('Maximum Slope vs Slip Ratio on Mars', fontsize=13)
axes[0].legend(fontsize=10)
axes[0].grid(True, alpha=0.3)
axes[0].set_xlim(5, 60)

# 右パネル: 各ローバーの質量対走行性能レーダー
# 正規化した比較指標
labels = ['Wheel Diameter', 'Mass Efficiency', 'Contact Area', 'Max Slope\n(s=0.2)']
n_labels = len(labels)
angles_radar = np.linspace(0, 2 * np.pi, n_labels, endpoint=False).tolist()
angles_radar += angles_radar[:1]

for name, spec in rovers.items():
    # 各指標を0-1に正規化
    d_norm = spec['r_wheel'] * 2 / 0.525  # Perseveranceを基準
    # 質量効率 = 車輪面積 / 車体質量
    area = spec['b_wheel'] * np.sqrt(2 * spec['r_wheel'] * 0.03) * spec['n_wheels']
    mass_eff = area / spec['mass'] * 1000
    mass_eff_norm = min(mass_eff / 5.0, 1.0)
    # 接地面積
    contact = spec['b_wheel'] * np.sqrt(2 * spec['r_wheel'] * 0.03)
    contact_norm = contact / 0.10  # 正規化

    ms_20 = compute_max_slope(
        spec['mass'], spec['n_wheels'], spec['r_wheel'], spec['b_wheel'],
        g_mars, c_soil, phi_soil, K_shear, kc, kphi, n_bekker, slip=0.2
    )
    slope_norm = ms_20 / 30.0

    values = [d_norm, mass_eff_norm, contact_norm, slope_norm]
    values += values[:1]

    axes[1].set_frame_on(False)

# 棒グラフで代替（レーダーはmatplotlibで煩雑なため）
axes[1].clear()
rover_names = list(rovers.keys())
metrics = {
    'Wheel diam [cm]': [rovers[n]['r_wheel'] * 200 for n in rover_names],
    'Max slope [deg]\n(slip=20%)': [],
}
for name in rover_names:
    spec = rovers[name]
    ms = compute_max_slope(
        spec['mass'], spec['n_wheels'], spec['r_wheel'], spec['b_wheel'],
        g_mars, c_soil, phi_soil, K_shear, kc, kphi, n_bekker, slip=0.2
    )
    metrics['Max slope [deg]\n(slip=20%)'].append(ms)

x = np.arange(len(rover_names))
width = 0.35

bars1 = axes[1].bar(x - width/2, metrics['Wheel diam [cm]'], width,
                     label='Wheel diameter [cm]', color='#42A5F5', alpha=0.8)
axes[1].set_ylabel('Wheel diameter [cm]', fontsize=11, color='#42A5F5')
axes[1].set_ylim(0, 60)

ax2 = axes[1].twinx()
bars2 = ax2.bar(x + width/2, metrics['Max slope [deg]\n(slip=20%)'], width,
                label='Max slope [deg]', color='#EF5350', alpha=0.8)
ax2.set_ylabel('Max climbable slope [deg]', fontsize=11, color='#EF5350')
ax2.set_ylim(0, 40)

axes[1].set_xticks(x)
axes[1].set_xticklabels(rover_names, rotation=25, ha='right', fontsize=9)
axes[1].set_title('Rover Comparison: Wheel Size vs Max Slope', fontsize=13)
axes[1].grid(True, alpha=0.2, axis='y')

fig.legend(loc='upper right', bbox_to_anchor=(0.98, 0.95), fontsize=10)
plt.tight_layout()
plt.savefig('rover_comparison.png', dpi=150, bbox_inches='tight')
plt.show()

import numpy as np
import matplotlib.pyplot as plt

# --- ローバーパラメータ（Curiosity相当） ---
mass = 899          # [kg]
n_wheels = 6
r_wheel = 0.25      # [m]
b_wheel = 0.40      # [m]
g_mars = 3.72       # [m/s^2]
I_wheel = 0.5       # [kg m^2] 車輪の慣性モーメント

# --- 土壌パラメータ ---
c_soil = 0.5e3      # [Pa]
phi_soil = np.radians(35)
K_shear = 0.02      # [m]
kc = 1.4e3
kphi = 820e3
n_bekker = 1.0

# --- 沈下深さ計算 ---
def compute_sinkage(W, b, r, kc, kphi, n):
    z0 = 0.01
    for _ in range(100):
        l = np.sqrt(2 * r * z0) if z0 > 0 else 0.01
        p_mean = W / (b * l)
        z0_new = (p_mean / (kc / b + kphi))**(1.0 / n)
        if abs(z0_new - z0) < 1e-6:
            break
        z0 = 0.5 * z0 + 0.5 * z0_new
    return z0

# --- 牽引力 ---
def traction(slip, W, b, r, z0, c, phi, K):
    l = np.sqrt(2 * r * z0)
    sigma = W / (b * l)
    tau_max = c + sigma * np.tan(phi)
    s_safe = max(abs(slip), 1e-10)
    F_D = b * l * tau_max * (1 - (K / (s_safe * l)) * (1 - np.exp(-s_safe * l / K)))
    return F_D * np.sign(slip) if slip != 0 else 0

# --- 圧密抵抗 ---
def compaction_resistance(z0, b, kc, kphi, n):
    return b * (kc / b + kphi) * z0**(n + 1) / (n + 1)

# --- シミュレーション ---
dt = 0.01           # 時間刻み [s]
t_max = 120         # シミュレーション時間 [s]
n_steps = int(t_max / dt)

# 3つの土壌条件でシミュレーション
soil_conditions = {
    'Hard soil (K=0.01)': {'K': 0.01, 'kphi_factor': 1.5},
    'Medium soil (K=0.02)': {'K': 0.02, 'kphi_factor': 1.0},
    'Soft soil (K=0.04)': {'K': 0.04, 'kphi_factor': 0.5},
}

fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)

for soil_name, soil_params in soil_conditions.items():
    K = soil_params['K']
    kphi_local = kphi * soil_params['kphi_factor']

    # モーターが出力する一定トルク [Nm]（1輪あたり）
    tau_motor = 8.0

    # 状態変数
    v = 0.0         # 車体前進速度 [m/s]
    omega = 0.0     # 車輪角速度 [rad/s]
    x_pos = 0.0     # 走行距離 [m]

    # 記録配列
    t_arr = np.zeros(n_steps)
    v_arr = np.zeros(n_steps)
    slip_arr = np.zeros(n_steps)
    x_arr = np.zeros(n_steps)

    W_per_wheel = mass * g_mars / n_wheels

    for i in range(n_steps):
        t = i * dt
        t_arr[i] = t

        # 沈下深さ（速度依存: 高スリップほど掘り込む）
        z0_base = compute_sinkage(W_per_wheel, b_wheel, r_wheel, kc, kphi_local, n_bekker)
        slip = (r_wheel * omega - v) / max(r_wheel * omega, 0.001)
        slip = np.clip(slip, 0, 0.99)
        # 高スリップで沈下が増大する効果を追加
        z0 = z0_base * (1 + 2.0 * slip**2)

        # 牽引力（1輪あたり）
        F_D_wheel = traction(slip, W_per_wheel, b_wheel, r_wheel, z0,
                             c_soil, phi_soil, K)

        # 圧密抵抗（1輪あたり）
        F_R_wheel = compaction_resistance(z0, b_wheel, kc, kphi_local, n_bekker)

        # 車体の運動方程式
        F_net_total = n_wheels * (F_D_wheel - F_R_wheel)
        a_body = F_net_total / mass
        v += a_body * dt
        v = max(v, 0)  # 後退しない
        x_pos += v * dt

        # 車輪の回転運動方程式
        # tau_motor = I_wheel * d(omega)/dt + r_wheel * F_D_wheel
        alpha_wheel = (tau_motor - r_wheel * F_D_wheel) / I_wheel
        omega += alpha_wheel * dt
        omega = max(omega, 0)

        v_arr[i] = v
        slip_arr[i] = slip
        x_arr[i] = x_pos

    axes[0].plot(t_arr, v_arr * 100, linewidth=2, label=soil_name)
    axes[1].plot(t_arr, slip_arr * 100, linewidth=2, label=soil_name)
    axes[2].plot(t_arr, x_arr, linewidth=2, label=soil_name)

axes[0].set_ylabel('Velocity [cm/s]', fontsize=12)
axes[0].set_title('Rover Driving Dynamics on Different Soils (Mars)', fontsize=13)
axes[0].legend(fontsize=10)
axes[0].grid(True, alpha=0.3)

axes[1].set_ylabel('Slip ratio [%]', fontsize=12)
axes[1].legend(fontsize=10)
axes[1].grid(True, alpha=0.3)

axes[2].set_xlabel('Time [s]', fontsize=12)
axes[2].set_ylabel('Distance [m]', fontsize=12)
axes[2].legend(fontsize=10)
axes[2].grid(True, alpha=0.3)

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

import numpy as np
import matplotlib.pyplot as plt

# --- ローバーパラメータ（Curiosity相当） ---
mass = 899
n_wheels = 6
r_wheel = 0.25
b_wheel = 0.40
g_mars = 3.72
I_wheel = 0.5

# --- 土壌パラメータ（柔らかい砂地） ---
c_soil = 0.5e3
phi_soil = np.radians(35)
K_shear = 0.04       # 柔らかい砂地
kc = 1.4e3
kphi = 820e3 * 0.5   # 柔らかい砂地
n_bekker = 1.0

# --- シミュレーション ---
dt = 0.01
t_max = 120
n_steps = int(t_max / dt)
W_per_wheel = mass * g_mars / n_wheels

def compute_sinkage(W, b, r, kc, kphi, n):
    z0 = 0.01
    for _ in range(100):
        l = np.sqrt(2 * r * z0) if z0 > 0 else 0.01
        p_mean = W / (b * l)
        z0_new = (p_mean / (kc / b + kphi))**(1.0 / n)
        if abs(z0_new - z0) < 1e-6:
            break
        z0 = 0.5 * z0 + 0.5 * z0_new
    return z0

def traction(slip, W, b, r, z0, c, phi, K):
    l = np.sqrt(2 * r * z0)
    sigma = W / (b * l)
    tau_max = c + sigma * np.tan(phi)
    s_safe = max(abs(slip), 1e-10)
    F_D = b * l * tau_max * (1 - (K / (s_safe * l)) * (1 - np.exp(-s_safe * l / K)))
    return F_D

def compaction_resistance(z0, b, kc, kphi, n):
    return b * (kc / b + kphi) * z0**(n + 1) / (n + 1)

def simulate_drive(use_slip_control=False, s_target=0.20, Kp=20.0, Kd=5.0):
    """走行シミュレーション（スリップ制御あり/なし）"""
    v = 0.0
    omega = 0.0
    x_pos = 0.0
    tau_motor_base = 8.0
    slip_prev = 0.0

    t_arr = np.zeros(n_steps)
    v_arr = np.zeros(n_steps)
    slip_arr = np.zeros(n_steps)
    x_arr = np.zeros(n_steps)
    z0_arr = np.zeros(n_steps)

    for i in range(n_steps):
        t = i * dt
        t_arr[i] = t

        z0_base = compute_sinkage(W_per_wheel, b_wheel, r_wheel, kc, kphi, n_bekker)
        slip = (r_wheel * omega - v) / max(r_wheel * omega, 0.001)
        slip = np.clip(slip, 0, 0.99)
        z0 = z0_base * (1 + 2.0 * slip**2)

        # スリップ制御
        if use_slip_control:
            slip_error = slip - s_target
            slip_rate = (slip - slip_prev) / dt
            tau_motor = tau_motor_base - Kp * slip_error - Kd * slip_rate
            tau_motor = np.clip(tau_motor, 0, tau_motor_base * 1.5)
        else:
            tau_motor = tau_motor_base

        F_D_wheel = traction(slip, W_per_wheel, b_wheel, r_wheel, z0,
                             c_soil, phi_soil, K_shear)
        F_R_wheel = compaction_resistance(z0, b_wheel, kc, kphi, n_bekker)

        F_net_total = n_wheels * (F_D_wheel - F_R_wheel)
        a_body = F_net_total / mass
        v += a_body * dt
        v = max(v, 0)
        x_pos += v * dt

        alpha_wheel = (tau_motor - r_wheel * F_D_wheel) / I_wheel
        omega += alpha_wheel * dt
        omega = max(omega, 0)

        v_arr[i] = v
        slip_arr[i] = slip
        x_arr[i] = x_pos
        z0_arr[i] = z0 * 100  # cm

        slip_prev = slip

    return t_arr, v_arr, slip_arr, x_arr, z0_arr

# 2つのケースをシミュレーション
t1, v1, s1, x1, z1 = simulate_drive(use_slip_control=False)
t2, v2, s2, x2, z2 = simulate_drive(use_slip_control=True)

fig, axes = plt.subplots(4, 1, figsize=(12, 12), sharex=True)

# 速度
axes[0].plot(t1, v1 * 100, 'r-', linewidth=2, label='No slip control')
axes[0].plot(t2, v2 * 100, 'b-', linewidth=2, label='With slip control (target=20%)')
axes[0].set_ylabel('Velocity [cm/s]', fontsize=12)
axes[0].set_title('Effect of Slip Control on Soft Sand', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3)

# スリップ率
axes[1].plot(t1, s1 * 100, 'r-', linewidth=2, label='No slip control')
axes[1].plot(t2, s2 * 100, 'b-', linewidth=2, label='With slip control')
axes[1].axhline(y=20, color='green', linestyle='--', alpha=0.5, label='Target slip (20%)')
axes[1].axhline(y=50, color='orange', linestyle='--', alpha=0.5, label='Danger zone (50%)')
axes[1].set_ylabel('Slip ratio [%]', fontsize=12)
axes[1].legend(fontsize=10)
axes[1].grid(True, alpha=0.3)

# 走行距離
axes[2].plot(t1, x1, 'r-', linewidth=2, label='No slip control')
axes[2].plot(t2, x2, 'b-', linewidth=2, label='With slip control')
axes[2].set_ylabel('Distance [m]', fontsize=12)
axes[2].legend(fontsize=11)
axes[2].grid(True, alpha=0.3)

# 沈下深さ
axes[3].plot(t1, z1, 'r-', linewidth=2, label='No slip control')
axes[3].plot(t2, z2, 'b-', linewidth=2, label='With slip control')
axes[3].set_xlabel('Time [s]', fontsize=12)
axes[3].set_ylabel('Sinkage [cm]', fontsize=12)
axes[3].legend(fontsize=11)
axes[3].grid(True, alpha=0.3)

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