311 lines
10 KiB
Python
311 lines
10 KiB
Python
import time
|
||
import krpc
|
||
import math
|
||
|
||
# ================= 配置参数 =================
|
||
TARGET_LANDING_VELOCITY = -2.0 # 目标着陆速度 (m/s)
|
||
SAFETY_MARGIN = 80.0 # 安全余量 (m)
|
||
MIN_TWR_FOR_BURN = 1.1 # 开始减速的最小TWR
|
||
|
||
# PID 控制器参数
|
||
SPEED_KP = 0.15
|
||
SPEED_KI = 0.05
|
||
SPEED_KD = 0.1
|
||
INTEGRAL_LIMIT = 0.5
|
||
|
||
# 节流阀滤波
|
||
ALPHA = 0.3 # 平滑系数
|
||
|
||
DT = 0.1 # 控制周期
|
||
|
||
# 预测参数
|
||
PREDICTION_STEPS = 50 # 预测步数
|
||
# ===========================================
|
||
|
||
conn = krpc.connect(name='Dynamic Landing V2')
|
||
if not conn.space_center:
|
||
print("No active vessel found.")
|
||
exit()
|
||
vessel = conn.space_center.active_vessel
|
||
|
||
class PID:
|
||
def __init__(self, kp, ki, kd, integral_limit=1.0):
|
||
self.kp, self.ki, self.kd = kp, ki, kd
|
||
self.prev_error = 0
|
||
self.integral = 0
|
||
self.integral_limit = integral_limit
|
||
|
||
def update(self, error, dt):
|
||
self.integral += error * dt
|
||
self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral))
|
||
derivative = (error - self.prev_error) / dt
|
||
self.prev_error = error
|
||
return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
|
||
|
||
class AdvancedRocketPerformance:
|
||
"""高级火箭性能计算器 - 考虑燃料消耗和TWR变化"""
|
||
|
||
def __init__(self, vessel):
|
||
self.vessel = vessel
|
||
self.body = vessel.orbit.body
|
||
self.g = self.body.surface_gravity # 重力加速度 (m/s²)
|
||
|
||
# 获取发动机参数
|
||
self.engines = vessel.parts.engines
|
||
self.max_thrust = sum(e.max_thrust for e in self.engines if e.active)
|
||
|
||
# 计算燃料消耗率 (kg/s) - 假设所有发动机都激活
|
||
self.fuel_flow_rate = 0.0
|
||
for engine in self.engines:
|
||
if engine.active:
|
||
# 获取发动机的比冲和推力
|
||
# fuel_flow = thrust / (Isp * g0)
|
||
# g0 = 9.80665 m/s²
|
||
isp = engine.vacuum_specific_impulse
|
||
self.fuel_flow_rate += engine.max_thrust / (isp * 9.80665)
|
||
|
||
def get_mass(self):
|
||
"""获取当前质量 (kg)"""
|
||
return self.vessel.mass
|
||
|
||
def get_fuel_mass(self):
|
||
"""获取燃料质量 (kg)"""
|
||
fuel_mass = 0.0
|
||
for part in self.vessel.parts.all:
|
||
for resource in part.resources:
|
||
if resource.name in ['LiquidFuel', 'Oxidizer', 'MonoPropellant']:
|
||
fuel_mass += resource.amount * resource.density
|
||
return fuel_mass
|
||
|
||
def get_dry_mass(self):
|
||
"""获取干重 (kg)"""
|
||
return self.get_mass() - self.get_fuel_mass()
|
||
|
||
def get_twr(self, mass=None):
|
||
"""
|
||
计算推重比 (Thrust-to-Weight Ratio)
|
||
|
||
Args:
|
||
mass: 可选，指定质量计算TWR。如果为None，使用当前质量
|
||
"""
|
||
if mass is None:
|
||
mass = self.get_mass()
|
||
if mass == 0:
|
||
return 0.0
|
||
return self.max_thrust / (mass * self.g)
|
||
|
||
def get_max_deceleration(self, mass=None):
|
||
"""
|
||
计算最大减速度 (m/s²)
|
||
|
||
Args:
|
||
mass: 可选，指定质量计算减速度。如果为None，使用当前质量
|
||
"""
|
||
twr = self.get_twr(mass)
|
||
# 有效减速度 = 推重比 * g - g = g * (TWR - 1)
|
||
return self.g * (twr - 1) if twr > 1 else 0.0
|
||
|
||
def predict_mass_after_burn(self, burn_time, throttle=1.0):
|
||
"""
|
||
预测燃烧指定时间后的质量
|
||
|
||
Args:
|
||
burn_time: 燃烧时间 (s)
|
||
throttle: 节流阀 (0-1)
|
||
|
||
Returns:
|
||
预测的质量 (kg)
|
||
"""
|
||
fuel_consumed = self.fuel_flow_rate * throttle * burn_time
|
||
current_fuel = self.get_fuel_mass()
|
||
remaining_fuel = max(0, current_fuel - fuel_consumed)
|
||
return self.get_dry_mass() + remaining_fuel
|
||
|
||
def calculate_deceleration_distance_advanced(self, current_velocity, target_velocity):
|
||
"""
|
||
计算从当前速度减速到目标速度需要的距离（考虑TWR变化）
|
||
|
||
使用数值积分方法，考虑燃料消耗导致的TWR增加
|
||
|
||
Args:
|
||
current_velocity: 当前垂直速度 (m/s, 负值表示下降)
|
||
target_velocity: 目标速度 (m/s, 负值表示下降)
|
||
|
||
Returns:
|
||
需要的减速距离 (m)
|
||
"""
|
||
# 检查当前是否能够减速
|
||
current_twr = self.get_twr()
|
||
if current_twr <= MIN_TWR_FOR_BURN:
|
||
return float('inf') # 无法减速
|
||
|
||
# 使用数值积分计算减速距离
|
||
# 将减速过程分成多个小段，每段使用该段的平均TWR
|
||
|
||
total_distance = 0.0
|
||
v = current_velocity
|
||
dt = 0.1 # 时间步长 (s)
|
||
|
||
while v < target_velocity:
|
||
# 预测当前速度下燃烧dt时间后的质量
|
||
# 假设平均节流阀为0.8（保守估计）
|
||
predicted_mass = self.predict_mass_after_burn(dt, throttle=0.8)
|
||
|
||
# 计算该段的平均减速度
|
||
avg_decel = self.get_max_deceleration(predicted_mass)
|
||
|
||
if avg_decel <= 0:
|
||
return float('inf') # 无法继续减速
|
||
|
||
# 计算该段的速度变化和距离
|
||
dv = avg_decel * dt
|
||
new_v = v + dv
|
||
|
||
# 使用平均速度计算距离
|
||
avg_v = (v + new_v) / 2
|
||
distance = abs(avg_v * dt)
|
||
|
||
total_distance += distance
|
||
v = new_v
|
||
|
||
# 防止无限循环
|
||
if total_distance > 100000: # 超过100km认为不合理
|
||
return float('inf')
|
||
|
||
return total_distance
|
||
|
||
def should_start_deceleration(self, altitude, current_velocity, target_velocity):
|
||
"""
|
||
判断是否应该开始减速（高级版）
|
||
|
||
Args:
|
||
altitude: 当前高度 (m)
|
||
current_velocity: 当前垂直速度 (m/s)
|
||
target_velocity: 目标速度 (m/s)
|
||
|
||
Returns:
|
||
(should_burn, required_distance, trigger_altitude, current_twr)
|
||
"""
|
||
# 计算需要的减速距离（考虑TWR变化）
|
||
required_distance = self.calculate_deceleration_distance_advanced(
|
||
current_velocity, target_velocity
|
||
)
|
||
|
||
# 添加安全余量
|
||
trigger_altitude = required_distance + SAFETY_MARGIN
|
||
|
||
# 检查TWR是否足够
|
||
current_twr = self.get_twr()
|
||
|
||
should_burn = (altitude <= trigger_altitude) and (current_twr >= MIN_TWR_FOR_BURN)
|
||
|
||
return should_burn, required_distance, trigger_altitude, current_twr
|
||
|
||
# 初始化控制器
|
||
speed_pid = PID(SPEED_KP, SPEED_KI, SPEED_KD, INTEGRAL_LIMIT)
|
||
performance = AdvancedRocketPerformance(vessel)
|
||
|
||
# 节流阀平滑处理
|
||
last_throttle = 0.0
|
||
|
||
# 姿态控制设置
|
||
vessel.auto_pilot.engage()
|
||
vessel.auto_pilot.reference_frame = vessel.surface_velocity_reference_frame
|
||
vessel.auto_pilot.target_direction = (0, -1, 0) # 始终指向速度反方向
|
||
|
||
print("高级动态着陆程序启动...")
|
||
print(f"目标着陆速度: {TARGET_LANDING_VELOCITY} m/s")
|
||
print(f"安全余量: {SAFETY_MARGIN} m")
|
||
print(f"最大推力: {performance.max_thrust:.0f} N")
|
||
print(f"燃料消耗率: {performance.fuel_flow_rate:.2f} kg/s")
|
||
|
||
# 自检
|
||
vessel.control.throttle = 0.99
|
||
time.sleep(0.1)
|
||
vessel.control.throttle = 0
|
||
|
||
burn_started = False
|
||
last_status = ""
|
||
|
||
while True:
|
||
# 1. 获取飞行数据
|
||
flight = vessel.flight(vessel.orbit.body.reference_frame)
|
||
alt = max(0, flight.surface_altitude - 9.50) # 考虑雷达高度计偏移
|
||
vel = flight.vertical_speed
|
||
|
||
# 2. 获取火箭性能
|
||
mass = performance.get_mass()
|
||
fuel_mass = performance.get_fuel_mass()
|
||
dry_mass = performance.get_dry_mass()
|
||
thrust = performance.max_thrust
|
||
twr = performance.get_twr()
|
||
max_decel = performance.get_max_deceleration()
|
||
|
||
# 3. 判断是否应该开始减速
|
||
should_burn, required_dist, trigger_alt, current_twr = performance.should_start_deceleration(
|
||
alt, vel, TARGET_LANDING_VELOCITY
|
||
)
|
||
|
||
# 4. 姿态控制逻辑
|
||
if alt < 30 or abs(vel) < 0.5:
|
||
# 离地很近或速度很慢时，强制垂直向上
|
||
vessel.auto_pilot.reference_frame = vessel.surface_reference_frame
|
||
vessel.auto_pilot.target_direction = (1, 0, 0) # 向天
|
||
else:
|
||
# 正常减速阶段，锁定速度反方向
|
||
vessel.auto_pilot.reference_frame = vessel.surface_velocity_reference_frame
|
||
vessel.auto_pilot.target_direction = (0, -1, 0)
|
||
|
||
# 5. 刹车控制
|
||
if alt < 10000 and thrust > 0 and not vessel.control.brakes:
|
||
vessel.control.brakes = True
|
||
|
||
if alt < 30 and vessel.control.brakes:
|
||
vessel.control.brakes = False
|
||
|
||
# 6. 自动部署脚架
|
||
if alt < 500:
|
||
vessel.control.gear = True
|
||
|
||
# 7. 动态速度规划
|
||
if should_burn:
|
||
burn_started = True
|
||
target_vel = TARGET_LANDING_VELOCITY
|
||
elif not burn_started:
|
||
# 还没开始减速，使用自由落体或微调
|
||
target_vel = -10000.0 # 允许快速下降
|
||
else:
|
||
# 已经开始减速，保持目标着陆速度
|
||
target_vel = TARGET_LANDING_VELOCITY
|
||
|
||
# 8. 触地检测
|
||
if alt < 1.0 and abs(vel) < 0.5:
|
||
vessel.control.throttle = 0
|
||
vessel.control.sas = True
|
||
print("已着陆！")
|
||
break
|
||
|
||
# 9. PID 计算
|
||
error = target_vel - vel
|
||
raw_throttle = speed_pid.update(error, DT)
|
||
|
||
# 10. 节流阀滤波
|
||
throttle = (ALPHA * raw_throttle) + ((1.0 - ALPHA) * last_throttle)
|
||
throttle = max(0.0, min(1.0, throttle))
|
||
last_throttle = throttle
|
||
|
||
# 11. 执行控制
|
||
vessel.control.throttle = throttle
|
||
|
||
# 12. 信息输出
|
||
if burn_started:
|
||
status = f"减速中 | TWR: {twr:.2f} | 燃料: {fuel_mass:.0f}kg"
|
||
else:
|
||
status = f"自由落体 | 触发高度: {trigger_alt:.0f}m"
|
||
|
||
print(f"高度: {alt:6.1f}m | 速度: {vel:6.1f}m/s | 目标速: {target_vel:6.1f}m/s | "
|
||
f"节流阀: {throttle:.2f} | 质量: {mass:.0f}kg | 减速距离: {required_dist:6.1f}m | "
|
||
f"{status}")
|
||
|
||
time.sleep(DT)
|