commit e251d681ab2138e4405c7d762f29735e644ce3c0 Author: kakune55 Date: Fri Jan 30 18:50:52 2026 +0800 feat: 初始化提交 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9d00186 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/pid.cpython-314.pyc diff --git a/pid.py b/pid.py new file mode 100644 index 0000000..dc17cb3 --- /dev/null +++ b/pid.py @@ -0,0 +1,33 @@ +class PID: + def __init__(self, kp, ki, kd, integral_limit=10.0, alpha=1.0, output_limit=(0.0, 1.0)): + self.kp = kp + self.ki = ki + self.kd = kd + self.prev_error = 0.0 + self.integral = 0.0 + self.integral_limit = integral_limit + self.alpha = alpha + self.prev_output = 0.0 + self.output_limit = output_limit + + def update(self, error, dt): + if dt <= 0: + dt = 1e-6 + self.integral += error * dt + self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral)) + derivative = (error - self.prev_error) / dt + output = self.kp * error + self.ki * self.integral + self.kd * derivative + self.prev_error = error + # 一阶滤波 + output = self.alpha * output + (1 - self.alpha) * self.prev_output + self.prev_output = output + + # 输出限制 + output = max(self.output_limit[0], min(self.output_limit[1], output)) + + return output + + def reset(self): + self.prev_error = 0.0 + self.integral = 0.0 + self.prev_output = 0.0 \ No newline at end of file diff --git a/参数获取测试.py b/参数获取测试.py new file mode 100644 index 0000000..a64137b --- /dev/null +++ b/参数获取测试.py @@ -0,0 +1,59 @@ +import time +import krpc +conn = krpc.connect(name='参数获取测试') +if not conn.space_center: + print("No active vessel found.") + exit() + +vessel = conn.space_center.active_vessel # 载具 +flight = vessel.flight(vessel.orbit.body.reference_frame) +celestial = vessel.orbit.body # 当前天体 + + +pressure = flight.static_pressure # 静压 + +gav = celestial.surface_gravity # 重力加速度 +available_thrust = vessel.available_thrust # 可用推力 + +twr = available_thrust / (vessel.mass * gav) # 推重比 + +print(pressure) +print(vessel.thrust) +print(vessel.available_thrust) +print(available_thrust) +print(gav) +print(twr) + +# hybrid_ref = conn.space_center.ReferenceFrame.create_hybrid( +# position=vessel.orbit.body.reference_frame, +# rotation=vessel.surface_reference_frame +# ) +# flight = vessel.flight(hybrid_ref) +# print("速度向量:", flight.velocity) +# print("航向(heading):", flight.heading, "°") # 你的仪表盘方向 +# print("俯仰(pitch):", flight.pitch, "°") # 正=抬头 +# print("垂直速:", flight.vertical_speed, "m/s") # 正=上 + +# while True: +# print(flight.velocity," "*10, end='\r') +# time.sleep(0.1) + + + +# 1. 世界坐标速度 (North, East, Up) —— 标准悬停/着陆用这个！ +world_ref = vessel.orbit.body.reference_frame +flight_world = vessel.flight(world_ref) + +vel = flight_world.velocity +vel_north = vel[0] # 北向 (正=北) +vel_east = vel[1] # 东向 (正=东，负=西) +vel_up = vel[2] # 向上 (正=爬升) + +print(f"世界速度: 北={vel_north:.2f}, 东={vel_east:.2f}, 上={vel_up:.2f} m/s") +print(f"垂直速确认: {flight_world.vertical_speed:.2f} m/s") # 应该 ≈ vel_up + +# 2. 姿态 (heading, pitch) —— 用 surface 或 hybrid 都行，但这里用 surface 更直接 +surface_ref = vessel.surface_reference_frame +flight_surface = vessel.flight(surface_ref) +print(f"航向: {flight_surface.heading:.1f}°") +print(f"俯仰: {flight_surface.pitch:.1f}°") \ No newline at end of file diff --git a/测试文件/动态着陆.py b/测试文件/动态着陆.py new file mode 100644 index 0000000..8a57f4c --- /dev/null +++ b/测试文件/动态着陆.py @@ -0,0 +1,237 @@ +import time +import krpc +import math + +# ================= 配置参数 ================= +TARGET_LANDING_VELOCITY = -2.0 # 目标着陆速度 (m/s) +SAFETY_MARGIN = 100.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 # 控制周期 +# =========================================== + +conn = krpc.connect(name='Dynamic Landing') +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 RocketPerformance: + """火箭性能计算器""" + + def __init__(self, vessel): + self.vessel = vessel + self.body = vessel.orbit.body + self.g = self.body.surface_gravity # 重力加速度 (m/s²) + + def get_mass(self): + """获取当前质量 (kg)""" + return self.vessel.mass + + def get_max_thrust(self): + """获取最大推力 (N)""" + return self.vessel.available_thrust + + def get_twr(self): + """计算推重比 (Thrust-to-Weight Ratio)""" + mass = self.get_mass() + thrust = self.get_max_thrust() + if mass == 0: + return 0.0 + return thrust / (mass * self.g) + + def get_max_deceleration(self): + """计算最大减速度 (m/s²)""" + twr = self.get_twr() + # 有效减速度 = 推重比 * g - g = g * (TWR - 1) + return self.g * (twr - 1) if twr > 1 else 0.0 + + def calculate_deceleration_distance(self, current_velocity, target_velocity): + """ + 计算从当前速度减速到目标速度需要的距离 + + 使用运动学公式: v² = v₀² + 2*a*d + 解得: d = (v² - v₀²) / (2*a) + + Args: + current_velocity: 当前垂直速度 (m/s, 负值表示下降) + target_velocity: 目标速度 (m/s, 负值表示下降) + + Returns: + 需要的减速距离 (m) + """ + # 如果速度为正值（上升），返回无穷大，表示不需要减速 + if current_velocity > 0: + return float('inf') + + # 如果目标速度是一个很大的负值（表示自由落体），返回无穷大 + if target_velocity < -1000: + return float('inf') + + # 获取当前的最大减速度（保守估计，使用平均TWR） + max_decel = self.get_max_deceleration() + + if max_decel <= 0: + return float('inf') # 无法减速 + + # 计算需要的距离 + # 注意：速度都是负值（下降），所以 v² - v₀² 是正值 + v0_sq = current_velocity ** 2 + v_sq = target_velocity ** 2 + + # 如果当前速度的绝对值小于目标速度的绝对值，说明已经足够慢了 + if abs(current_velocity) <= abs(target_velocity): + return 0.0 + + distance = (v_sq - v0_sq) / (2 * max_decel) + + return max(0, distance) + + def should_start_deceleration(self, altitude, current_velocity, target_velocity): + """ + 判断是否应该开始减速 + + Args: + altitude: 当前高度 (m) + current_velocity: 当前垂直速度 (m/s) + target_velocity: 目标速度 (m/s) + + Returns: + 是否应该开始减速 + """ + # 计算需要的减速距离 + required_distance = self.calculate_deceleration_distance( + current_velocity, target_velocity + ) + + # 添加安全余量 + trigger_altitude = required_distance + SAFETY_MARGIN + + # 检查TWR是否足够 + twr = self.get_twr() + + should_burn = (altitude <= trigger_altitude) and (twr >= MIN_TWR_FOR_BURN) + + return should_burn, required_distance, trigger_altitude, twr + +# 初始化控制器 +speed_pid = PID(SPEED_KP, SPEED_KI, SPEED_KD, INTEGRAL_LIMIT) +performance = RocketPerformance(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") + +# 自检 +vessel.control.throttle = 0.99 +time.sleep(0.1) +vessel.control.throttle = 0 + +burn_started = False + +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() + thrust = performance.get_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. 信息输出 + status = "减速中" if burn_started else "自由落体" + print(f"高度: {alt:6.1f}m | 速度: {vel:6.1f}m/s | 目标速: {target_vel:6.1f}m/s | " + f"节流阀: {throttle:.2f} | TWR: {twr:.2f} | 减速距离: {required_dist:6.1f}m | " + f"触发高度: {trigger_alt:6.1f}m | {status}") + + time.sleep(DT) diff --git a/测试文件/动态着陆v2.py b/测试文件/动态着陆v2.py new file mode 100644 index 0000000..9f20001 --- /dev/null +++ b/测试文件/动态着陆v2.py @@ -0,0 +1,310 @@ +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) diff --git a/测试文件/动态着陆v4.py b/测试文件/动态着陆v4.py new file mode 100644 index 0000000..15fc9e8 --- /dev/null +++ b/测试文件/动态着陆v4.py @@ -0,0 +1,37 @@ +import time +import krpc, simple_pid + + +conn = krpc.connect(name='动态着陆 V4') +if not conn.space_center: + print("No active vessel found.") + exit() +vessel = conn.space_center.active_vessel +flight = vessel.flight(vessel.orbit.body.reference_frame) +# ================= 配置参数 (V4 - 动态着陆) ================= + + + +# ================= 飞控 ================= + +pid = simple_pid.PID(0.15, 0.05, 0.1, setpoint=0) +pid.output_limits = (0, 1) # 节流阀范围 +alpha = 0.3 # 节流阀平滑系数 +last_throttle = 0.0 +print("动态着陆 V4 已启动") +while True: + alt = max(0, flight.surface_altitude - 9.50) # 考虑雷达高度计偏差 + vel = flight.speed + + # 计算 PID 输出 + raw_throttle = pid(vel) + if raw_throttle is None: + raw_throttle = 0.0 + + # 节流阀平滑处理 + smoothed_throttle = (1 - alpha) * last_throttle + alpha * raw_throttle + last_throttle = smoothed_throttle + + vessel.control.throttle = smoothed_throttle + + time.sleep(0.1) diff --git a/测试文件/动态着陆使用说明.md b/测试文件/动态着陆使用说明.md new file mode 100644 index 0000000..71990cd --- /dev/null +++ b/测试文件/动态着陆使用说明.md @@ -0,0 +1,547 @@ +# 动态着陆程序使用说明 + +## 概述 + +本程序实现了基于火箭性能的动态减速着陆算法，能够根据火箭的实时推重比（TWR）自动计算最佳减速时机，实现精准、高效的着陆。 + +## 核心特性 + +### 1. 动态减速时机计算 +- 根据火箭实时 TWR 自动计算最佳减速时机 +- 适应不同火箭配置和燃料状态 +- 优化燃料使用效率 + +### 2. 动态目标速度（V3） +- 根据高度和性能实时计算最优目标速度 +- 高空时允许快速下降 +- 低空时逐渐减速 +- 接近地面时达到着陆速度 + +### 3. 水平控制（V3） +- 主动修正水平速度 +- 双环控制：位置环 + 速度环 +- 确保火箭能够垂直着陆 + +### 4. 防自转优化（V3） +- 智能姿态控制策略 +- 防止点火时自转浪费推力 +- 在低速时使用稳定的参考系 + +## 核心原理 + +### 1. 推重比（TWR）计算 + +``` +TWR = 推力 / (质量 × 重力加速度) +``` + +- TWR > 1：火箭能够减速 +- TWR 越大，减速能力越强，可以更晚开始减速 + +### 2. 减速距离计算 + +使用运动学公式计算从当前速度减速到目标速度需要的距离： + +``` +v² = v₀² + 2 × a × d + +解得：d = (v² - v₀²) / (2 × a) +``` + +其中： +- v₀：当前速度 +- v：目标速度 +- a：有效减速度 = g × (TWR - 1) +- d：需要的减速距离 + +### 3. 动态目标速度计算（V3） + +根据当前高度和火箭性能，动态计算最优的目标速度： + +``` +v_target = -√(2 × a × h) × 安全系数 +``` + +其中： +- a：最大减速度 +- h：当前高度 +- 安全系数：0.8（确保有足够的减速能力） + +### 4. 水平控制（V3） + +使用双环控制策略： + +**位置环：** +``` +目标水平速度 = 位置误差 × POS_KP +``` + +**速度环：** +``` +倾角 = (目标水平速度 - 实际水平速度) × VEL_KP +``` + +## 版本对比 + +### 版本选择指南 + +| 版本 | 适用场景 | 特点 | 推荐度 | +|------|---------|------|--------| +| **动态着陆.py (V1)** | 简单场景，燃料充足 | 使用当前TWR，计算快速 | ⭐⭐⭐ | +| **动态着陆v2.py (V2)** | 复杂场景，燃料消耗显著 | 考虑燃料消耗和TWR变化 | ⭐⭐⭐⭐ | +| **动态着陆v3.py (V3)** | **推荐使用** | 动态目标速度 + 水平控制 + 防自转 | ⭐⭐⭐⭐⭐ | + +### V1 - 基础版 + +**文件：** [`动态着陆.py`](动态着陆.py) + +**特点：** +- 使用当前 TWR 计算减速距离 +- 简单直接，计算快速 +- 适用于 TWR 变化不大的情况 + +**配置参数：** +```python +TARGET_LANDING_VELOCITY = -2.0 # 目标着陆速度 (m/s) +SAFETY_MARGIN = 100.0 # 安全余量 (m) +MIN_TWR_FOR_BURN = 1.1 # 开始减速的最小TWR +``` + +**适用场景：** +- 火箭燃料充足，燃料消耗对 TWR 影响不大 +- 需要快速响应，计算延迟敏感 +- 火箭质量相对稳定 +- 简单场景，不需要高精度 + +### V2 - 进阶版 + +**文件：** [`动态着陆v2.py`](动态着陆v2.py) + +**特点：** +- 考虑燃料消耗导致的 TWR 变化 +- 使用数值积分精确计算减速距离 +- 预测燃烧过程中的质量变化 +- 更精确，适用于燃料消耗显著的情况 + +**配置参数：** +```python +TARGET_LANDING_VELOCITY = -2.0 # 目标着陆速度 (m/s) +SAFETY_MARGIN = 80.0 # 安全余量 (m) +MIN_TWR_FOR_BURN = 1.1 # 开始减速的最小TWR +PREDICTION_STEPS = 50 # 预测步数 +``` + +**适用场景：** +- 火箭燃料消耗显著（燃料占比 > 30%） +- 需要高精度着陆 +- 火箭 TWR 变化明显 +- 复杂场景，需要精确控制 + +### V3 - 智能版（推荐） + +**文件：** [`动态着陆v3.py`](动态着陆v3.py) + +**特点：** +- **动态目标速度计算**：根据高度和性能实时计算最优目标速度 +- **水平控制**：主动修正水平速度，确保垂直着陆 +- **防自转优化**：智能姿态控制，防止点火时自转浪费推力 +- 最智能的减速策略 +- 自动适应不同火箭配置 +- 最佳的燃料效率 + +**配置参数：** +```python +# 垂直控制参数 +TARGET_LANDING_VELOCITY = -2.0 # 目标着陆速度 (m/s) +SAFETY_MARGIN = 100.0 # 安全余量 (m) +MIN_TWR_FOR_BURN = 1.1 # 开始减速的最小TWR + +# 水平控制参数 +POS_KP = 0.04 # 位置环增益 +MAX_H_VEL = 15.0 # 最大水平速度 +VEL_KP = 0.12 # 速度环增益 +VEL_KI = 0.01 +VEL_KD = 0.80 +MAX_TILT = 0.50 # 最大倾角（弧度，约28.6度） +``` + +**适用场景：** +- **所有场景（推荐）** +- 需要最佳燃料效率 +- 需要最智能的减速策略 +- 需要水平控制 +- 自动适应不同火箭配置 + +## 使用方法 + +### 1. 准备工作 + +确保： +- KSP 游戏已启动 +- kRPC 服务器已运行 +- 火箭已发射并处于下降阶段 +- 火箭有足够的燃料完成着陆 + +### 2. 运行程序 + +**推荐使用 V3 版本：** +```bash +python 动态着陆v3.py +``` + +**其他版本：** +```bash +# V1 - 基础版 +python 动态着陆.py + +# V2 - 进阶版 +python 动态着陆v2.py +``` + +### 3. 程序输出（V3） + +程序会实时输出以下信息： +- 高度（m） +- 垂直速度（m/s） +- 水平速度（m/s） +- 目标速度（m/s） +- 节流阀（0-1） +- TWR（推重比） +- 距离目标点的距离（m） +- 减速距离（m） +- 触发高度（m） +- 当前状态（自由落体/减速中） + +**示例输出（V3）：** +``` +高度: 5000.0m | 垂直速: -150.0m/s | 水平速: 10.5m/s | 目标速: -200.0m/s | 节流阀: 0.00 | TWR: 1.50 | 距离: 500.0m | 减速距离: 500.0m | 触发高度: 600.0m | 自由落体 +高度: 600.0m | 垂直速: -150.0m/s | 水平速: 5.2m/s | 目标速: -50.0m/s | 节流阀: 0.85 | TWR: 1.55 | 距离: 100.0m | 减速距离: 520.0m | 触发高度: 620.0m | 减速中 +高度: 100.0m | 垂直速: -20.0m/s | 水平速: 1.5m/s | 目标速: -5.0m/s | 节流阀: 0.30 | TWR: 1.65 | 距离: 10.0m | 减速距离: 50.0m | 触发高度: 150.0m | 减速中 +高度: 5.0m | 垂直速: -2.5m/s | 水平速: 0.2m/s | 目标速: -2.0m/s | 节流阀: 0.10 | TWR: 1.70 | 距离: 1.0m | 减速距离: 5.0m | 触发高度: 105.0m | 减速中 +已着陆！ +``` + +## 参数调整建议 + +### 1. 目标着陆速度（TARGET_LANDING_VELOCITY） + +- **推荐值：-2.0 m/s** +- 范围：-1.0 到 -5.0 m/s +- 影响因素： + - 起落架强度 + - 地面硬度 + - 精度要求 + +### 2. 安全余量（SAFETY_MARGIN） + +- **推荐值：80-100 m** +- V1：100 m（保守） +- V2/V3：100 m（更精确） +- 影响因素： + - 控制延迟 + - 燃料消耗不确定性 + - 大气扰动 + +### 3. 最小 TWR（MIN_TWR_FOR_BURN） + +- **推荐值：1.1** +- 范围：1.05 到 1.2 +- 影响： + - 值越小，开始减速越早（更安全） + - 值越大，开始减速越晚（更高效） + +### 4. 水平控制参数（V3） + +**位置环增益（POS_KP）：** +- **推荐值：0.04** +- 范围：0.02 到 0.08 +- 影响： + - 值越大，水平修正越快 + - 值越小，水平修正越平滑 + +**最大水平速度（MAX_H_VEL）：** +- **推荐值：15.0 m/s** +- 范围：10.0 到 20.0 m/s +- 影响： + - 限制最大水平速度 + - 防止过大的倾角 + +**速度环参数（VEL_KP, VEL_KI, VEL_KD）：** +- **推荐值：VEL_KP=0.12, VEL_KI=0.01, VEL_KD=0.80** +- 影响： + - 控制水平速度的响应速度和稳定性 + +**最大倾角（MAX_TILT）：** +- **推荐值：0.50（约28.6度）** +- 范围：0.30 到 0.70 +- 影响： + - 限制最大倾角 + - 防止过度倾斜 + +### 5. PID 参数 + +根据火箭特性调整： + +**轻型火箭（质量小，响应快）：** +```python +SPEED_KP = 0.20 +SPEED_KI = 0.08 +SPEED_KD = 0.15 +``` + +**重型火箭（质量大，响应慢）：** +```python +SPEED_KP = 0.10 +SPEED_KI = 0.03 +SPEED_KD = 0.08 +``` + +## 算法优势 + +### 1. 自适应性强 + +- 根据实时 TWR 自动调整减速时机 +- 适应不同火箭配置 +- 适应不同燃料状态 + +### 2. 燃料效率高 + +- 不会过早开始减速（浪费燃料） +- 不会过晚开始减速（无法减速） +- 优化减速曲线 + +### 3. 安全可靠 + +- 内置安全余量 +- TWR 检查机制 +- 多重保护措施 + +### 4. 精度高 + +- 基于物理公式计算 +- 考虑实际性能参数 +- 实时反馈调整 + +### 5. 智能化（V3） + +- 动态目标速度计算 +- 水平控制 +- 防自转优化 +- 自动适应不同高度 +- 最优减速策略 + +## 常见问题 + +### Q1: 为什么火箭会提前开始减速？ + +**可能原因：** +- 安全余量设置过大 +- 最小 TWR 设置过高 +- 火箭实际推力低于预期 + +**解决方法：** +- 减小 `SAFETY_MARGIN` +- 降低 `MIN_TWR_FOR_BURN` +- 检查发动机配置 + +### Q2: 为什么火箭会过晚开始减速？ + +**可能原因：** +- 安全余量设置过小 +- 燃料消耗导致 TWR 变化未考虑 +- 计算误差 + +**解决方法：** +- 增大 `SAFETY_MARGIN` +- 使用 V2 或 V3 版本（考虑燃料消耗） +- 检查发动机参数 + +### Q3: 为什么点火时火箭会自转？ + +**可能原因：** +- 使用了不稳定的参考系（`surface_velocity_reference_frame`） +- 速度很小时参考系不稳定 +- 姿态控制逻辑不完善 + +**解决方法：** +- **使用 V3 版本**（已内置防自转优化） +- 在低速时使用地表参考系 +- 在减速阶段且高度较低时保持垂直 + +### Q4: 如何提高着陆精度？ + +**方法：** +1. 使用 V3 版本 +2. 调整 PID 参数 +3. 减小目标着陆速度 +4. 增加安全余量 +5. 优化水平控制参数 + +### Q5: 如何节省燃料？ + +**方法：** +1. 减小安全余量（在可接受范围内） +2. 使用 V3 版本（最智能的减速策略） +3. 优化减速曲线 +4. 提前部署刹车 + +### Q6: 如何调整水平控制？ + +**方法：** +1. 调整 `POS_KP`：控制位置环的响应速度 +2. 调整 `MAX_H_VEL`：限制最大水平速度 +3. 调整 `VEL_KP, VEL_KI, VEL_KD`：优化速度环性能 +4. 调整 `MAX_TILT`：限制最大倾角 + +### Q7: V3 版本的优势是什么？ + +**V3 版本的优势：** +1. **动态目标速度**：根据高度和性能实时计算最优目标速度 +2. **水平控制**：主动修正水平速度，确保垂直着陆 +3. **防自转优化**：智能姿态控制，防止点火时自转浪费推力 +4. **最智能**：自动适应不同火箭配置和飞行状态 +5. **最高效**：最佳的燃料利用效率 +6. **最安全**：多重安全机制和自适应策略 + +**推荐使用 V3 版本！** + +## 技术细节 + +### 运动学公式 + +减速距离计算基于匀变速直线运动： + +``` +v² = v₀² + 2ad + +其中： +- v：最终速度 +- v₀：初始速度 +- a：加速度（减速度为负） +- d：位移 +``` + +### 有效减速度 + +考虑重力的影响： + +``` +有效减速度 = 推力加速度 - 重力加速度 + = (推力 / 质量) - g + = g × (TWR - 1) +``` + +### TWR 变化影响 + +随着燃料消耗： +- 质量减小 +- TWR 增大 +- 减速能力增强 +- 可以更晚开始减速 + +V2 和 V3 版本通过数值积分考虑了这个变化。 + +### 动态目标速度（V3） + +V3 版本使用动态目标速度算法： + +``` +v_target = -√(2 × a × h) × 安全系数 + +其中： +- a：最大减速度 +- h：当前高度 +- 安全系数：0.8 +``` + +这个公式确保： +- 高空时允许快速下降 +- 低空时逐渐减速 +- 接近地面时达到着陆速度 + +### 防自转优化（V3） + +V3 版本使用智能姿态控制策略： + +1. **低速或低高度时**：使用地表参考系，保持垂直 +2. **减速阶段且高度较低时**：使用地表参考系，应用水平控制 +3. **正在减速且节流阀较大时**：使用速度反方向 +4. **其他情况**：使用地表参考系，应用水平控制 + +这样可以防止在低速时使用速度参考系导致自转。 + +### 水平控制（V3） + +V3 版本使用双环控制策略： + +**位置环：** +``` +目标水平速度 = 位置误差 × POS_KP +``` + +**速度环：** +``` +倾角 = (目标水平速度 - 实际水平速度) × VEL_KP +``` + +这样可以实现： +- 主动修正水平速度 +- 确保火箭能够垂直着陆 +- 平滑的水平控制 + +## 扩展功能 + +可以添加的功能： + +1. **多阶段减速策略** + - 高速段：大倾角减速 + - 中速段：中等倾角 + - 低速段：垂直着陆 + +2. **目标点选择** + - 支持多个预设着陆点 + - 实时选择最优着陆点 + +3. **自适应参数调整** + - 根据着陆效果自动调整参数 + - 机器学习优化 + +4. **故障检测与恢复** + - 发动机故障检测 + - 备用着陆点选择 + +5. **实时可视化** + - 显示减速曲线 + - 显示预测轨迹 + - 显示性能参数 + +## 版本历史 + +- **v1.0（动态着陆.py）**：基础版本，使用当前 TWR +- **v2.0（动态着陆v2.py）**：进阶版本，考虑燃料消耗和 TWR 变化 +- **v3.0（动态着陆v3.py）**：智能版本，动态目标速度 + 水平控制 + 防自转优化（推荐） + +## 参考资料 + +- kRPC 文档：https://krpc.github.io/krpc/ +- Kerbal Space Program：https://www.kerbalspaceprogram.com/ +- SpaceX 着陆技术：https://www.spacex.com/ + +## 总结 + +本程序提供了三个版本的动态着陆算法： + +1. **V1（基础版）**：简单直接，适用于简单场景 +2. **V2（进阶版）**：考虑燃料消耗，适用于复杂场景 +3. **V3（智能版）**：动态目标速度 + 水平控制 + 防自转优化，推荐使用 + +**推荐使用 V3 版本（动态着陆v3.py）**，它具有： +- 最智能的减速策略 +- 最佳的燃料效率 +- 水平控制功能 +- 防自转优化 +- 自动适应不同火箭配置 + +--- + +**祝您着陆成功！** 🚀 diff --git a/测试文件/参考系可视化.py b/测试文件/参考系可视化.py new file mode 100644 index 0000000..93f4a83 --- /dev/null +++ b/测试文件/参考系可视化.py @@ -0,0 +1,25 @@ +import time +import krpc + +conn = krpc.connect(name='Orbital speed') +if not conn.space_center: + print("No active vessel found.") + exit() +vessel = conn.space_center.active_vessel +hybrid_ref = conn.space_center.ReferenceFrame.create_hybrid( + position=vessel.orbit.body.reference_frame, + rotation=vessel.surface_reference_frame +) # type: ignore +# 参考系为 (上, 北, 东) + +flight_surface = vessel.flight(hybrid_ref) + +drawing = conn.drawing +if not drawing: + print("Drawing service not available.") + exit() +drawing.add_direction_from_com((1, 0, 0), hybrid_ref, length=15) +drawing.add_direction_from_com((0, 1, 0), hybrid_ref, length=30) +drawing.add_direction_from_com((0, 0, 1), hybrid_ref, length=45) +while True: + pass diff --git a/测试文件/悬停+AutoPilot.py b/测试文件/悬停+AutoPilot.py new file mode 100644 index 0000000..7c268fc --- /dev/null +++ b/测试文件/悬停+AutoPilot.py @@ -0,0 +1,136 @@ +import time +import krpc +import math + +# ================= 配置参数 (V5 - 针对稳定性与精准度重置) ================= +TARGET_ALTITUDE = 50.0 + +# 垂直控制 (双环) +ALT_OUTER_KP = 0.3 # 适度增大外环增益，平衡响应速度与稳定性 +ALT_INNER_KP = 0.05 # 适度增大速度环增益 +ALT_INNER_KI = 0.05 # 增大积分项，消除稳态误差 +ALT_INNER_KD = 0.25 # 增大阻尼，抑制过冲 + +# 水平控制 (双环 - 极端提前减速 + 暴力制动版) +POS_KP = 0.04 # 目标速 = 距离 * 0.04 (375m处就开始限制速度，实现极早制动) +MAX_H_VEL = 15.0 + +VEL_KP = 0.12 # 增强力度，让姿态更果断地跟上速度变化 +VEL_KI = 0.01 +VEL_KD = 0.80 # 极强阻尼，防止因大角度刹车导致的过冲和摆动 +MAX_TILT = 0.50 # 允许最大倾角约 28 度！提供 SpaceX 级别的暴力水平制动力 + +DT = 0.1 # 采样周期 +ALPHA = 0.3 # 节流阀滤波 (越小越平滑) +# ========================================================================= + +conn = krpc.connect(name='Hover Guidance V5') +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, limit=1.0): + self.kp, self.ki, self.kd = kp, ki, kd + self.limit = limit + self.integral = 0 + self.prev_error = 0 + + def update(self, error, dt): + self.integral = max(-self.limit, min(self.limit, self.integral + error * dt)) + derivative = (error - self.prev_error) / dt + self.prev_error = error + return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative) + +# 初始化所有控制器 +alt_pid = PID(ALT_INNER_KP, ALT_INNER_KI, ALT_INNER_KD, limit=1.0) +vel_n_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1) +vel_e_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1) + +# 初始化位置环积分 +pos_n_integral = 0.0 +pos_e_integral = 0.0 + +# 给定目标点 (KSC 着陆台) +target_lat = -0.097179090928 +target_lon = -74.5576787510 + +# # VBA房顶 +# target_lat = -0.09684801697319373 +# target_lon = -74.61749199803717 + + +body_radius = vessel.orbit.body.equatorial_radius +deg_to_m = (2 * math.pi * body_radius) / 360.0 + +# 准备状态 +vessel.auto_pilot.engage() +vessel.auto_pilot.reference_frame = vessel.surface_reference_frame +last_throttle = 0.0 + +print(f"制导系统 V5 已启动，目标高度: {TARGET_ALTITUDE}m") + +while True: + # 1. 采集速度数据 (使用 surface_velocity_reference_frame 获取地表相对速度) + v_surf = vessel.velocity(vessel.surface_velocity_reference_frame) + v_up = v_surf[0] + v_north = v_surf[1] + v_east = v_surf[2] + + flight = vessel.flight(vessel.surface_reference_frame) + alt = flight.surface_altitude + lat = flight.latitude + lon = flight.longitude + + # 2. 水平层逻辑 (制导) + # 计算位移误差（米） + err_n = (target_lat - lat) * deg_to_m + err_e = (target_lon - lon) * deg_to_m * math.cos(math.radians(lat)) + + # 调试信息打印 (提高精度到小数点后 6 位，显示微小速度变化) + dist = math.sqrt(err_n**2 + err_e**2) + + # 【核心重构】：线性减速模型。 + # 我们不再使用复杂的五段式，而是使用一个平滑的比例： + # 目标速度 = 距离 * 0.05。 + # 比如在 100m 处，目标速度就是 5m/s。 + # 在 20m 处，目标速度就是 1m/s。 + # 这样火箭从 300m 外就会开始感觉到“目标速度在变小”，从而实现极早的反向机动。 + target_vn = max(-MAX_H_VEL, min(MAX_H_VEL, err_n * POS_KP)) + target_ve = max(-MAX_H_VEL, min(MAX_H_VEL, err_e * POS_KP)) + + print(f"距离: {dist:.1f}m | 目标速: {math.sqrt(target_vn**2+target_ve**2):.1f} | N速: {v_north:.3f}") + + # 内环: 目标速 - 实速 -> 倾角 + tilt_n = vel_n_pid.update(target_vn - v_north, DT) + tilt_e = vel_e_pid.update(target_ve - v_east, DT) + + # 限制并设置姿态 + tilt_n = max(-MAX_TILT, min(MAX_TILT, tilt_n)) + tilt_e = max(-MAX_TILT, min(MAX_TILT, tilt_e)) + vessel.auto_pilot.target_direction = (1.0, tilt_n, tilt_e) + + # 3. 垂直层逻辑 + # 外环: 高度误差 -> 目标垂直速 (P) + target_vs = (TARGET_ALTITUDE - alt) * ALT_OUTER_KP + target_vs = max(-10.0, min(10.0, target_vs)) + + # 内环: 速度误差 -> 节流阀 (PID) + raw_throttle = alt_pid.update(target_vs - v_up, DT) + + # 加权补偿 (抵消因垂直向量偏置导致的升力损失) + cos_tilt = math.cos(math.sqrt(tilt_n**2 + tilt_e**2)) + throttle_corr = raw_throttle / max(0.8, cos_tilt) + + # 4. 滤波输出 + throttle = (ALPHA * throttle_corr) + ((1.0 - ALPHA) * last_throttle) + throttle = max(0.0, min(1.0, throttle)) + vessel.control.throttle = throttle + last_throttle = throttle + + # 5. 信息反馈 + dist = math.sqrt(err_n**2 + err_e**2) + print(f"距离: {dist:.2f}m | 高度: {alt:.1f}m | N_速: {v_north:.2f}m/s | 节流阀: {throttle:.2f}") + + time.sleep(DT) diff --git a/测试文件/悬停PID.py b/测试文件/悬停PID.py new file mode 100644 index 0000000..e4e7f6d --- /dev/null +++ b/测试文件/悬停PID.py @@ -0,0 +1,83 @@ +import time +import krpc +conn = krpc.connect(name='SpaceX Landing') +if not conn.space_center: + print("No active vessel found.") + exit() + +class PID: + def __init__(self, kp, ki, kd, integral_limit=10.0, alpha=1.0, output_limit=(0.0, 1.0)): + self.kp = kp + self.ki = ki + self.kd = kd + self.prev_error = 0.0 + self.integral = 0.0 + self.integral_limit = integral_limit + self.alpha = alpha + self.prev_output = 0.0 + self.output_limit = output_limit + + def update(self, error, dt): + if dt <= 0: + dt = 1e-6 + self.integral += error * dt + self.integral = max(-self.integral_limit, min(self.integral_limit, self.integral)) + derivative = (error - self.prev_error) / dt + output = self.kp * error + self.ki * self.integral + self.kd * derivative + self.prev_error = error + # 一阶滤波 + output = self.alpha * output + (1 - self.alpha) * self.prev_output + self.prev_output = output + + # 输出限制 + output = max(self.output_limit[0], min(self.output_limit[1], output)) + + return output + + +# control = vessel.control +vessel = conn.space_center.active_vessel + +# 创建 PID 控制器 +# 外环：高度 -> 目标垂直速度（允许负值） +altitude_pid = PID(kp=0.3, ki=0.03, kd=0.05, integral_limit=2.0, alpha=0.8, output_limit=(-10.0, 10.0)) + +# 内环：垂直速度 -> 节流阀（0~1） +speed_pid = PID(kp=0.12, ki=0.05, kd=0.05, integral_limit=50.0, alpha=0.5, output_limit=(0.0, 1.0)) + +target_altitude = 30 # 目标高度，单位 m + +last_time = time.monotonic() +loop_dt = 0.1 + +try: + while True: + now = time.monotonic() + dt = now - last_time + last_time = now + + flight = vessel.flight(vessel.orbit.body.reference_frame) + current_altitude = flight.surface_altitude + current_vertical_speed = flight.vertical_speed + + # 外环：高度误差 -> 目标垂直速度 + altitude_error = target_altitude - current_altitude + target_vertical_speed = altitude_pid.update(altitude_error, dt) + target_vertical_speed = max(-10.0, min(10.0, target_vertical_speed)) + + # 内环：垂直速度误差 -> 节流阀 + speed_error = target_vertical_speed - current_vertical_speed + throttle = speed_pid.update(speed_error, dt) + throttle = max(0.0, min(1.0, throttle)) + + vessel.control.throttle = throttle + print(f"当前高度: {current_altitude:.2f} m, 目标速: {target_vertical_speed:.2f} m/s, 速: {current_vertical_speed:.2f} m/s, 节流阀: {throttle:.2f}"," "*10, end='\r') + + # 保持大致的控制周期 + sleep_time = loop_dt - (time.monotonic() - now) + if sleep_time > 0: + time.sleep(sleep_time) + +except KeyboardInterrupt: + vessel.control.throttle = 0.0 + print("已停止控制，节流设为 0。") \ No newline at end of file diff --git a/测试文件/着陆.py b/测试文件/着陆.py new file mode 100644 index 0000000..daa0b2b --- /dev/null +++ b/测试文件/着陆.py @@ -0,0 +1,110 @@ +import time +import krpc + +conn = krpc.connect(name='SpaceX Landing') +if not conn.space_center: + print("No active vessel found.") + exit() +vessel = conn.space_center.active_vessel + +# 复用之前的 PID 类 +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) + +# 1. 初始设置 +vessel.auto_pilot.engage() +vessel.auto_pilot.reference_frame = vessel.surface_velocity_reference_frame +vessel.auto_pilot.target_direction = (0, -1, 0) # 始终指向速度反方向（Retrograde） + +speed_pid = PID(kp=0.15, ki=0.05, kd=0.1, integral_limit=0.5) + +# 节流阀平滑处理相关变量 +last_throttle = 0.0 +alpha = 0.3 # 平滑系数。越大响应越快，越小越平滑 + +print("着陆程序启动...") + +vessel.control.throttle = 0.99 +time.sleep(0.1) +vessel.control.throttle = 0 # 自检 + + +while True: + flight = vessel.flight(vessel.orbit.body.reference_frame) + # 获取距地高度（考虑到雷达高度计） + alt = max(0, flight.surface_altitude - 9.50) + vel = flight.vertical_speed + + # --- 姿态控制逻辑优化 --- + # 防止低速时由于速度方向频繁改变导致的姿态抖动 + 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: + # 正常减速阶段，锁定速度反方向（Retrograde） + vessel.auto_pilot.reference_frame = vessel.surface_velocity_reference_frame + vessel.auto_pilot.target_direction = (0, -1, 0) + + # 打开刹车 + if alt < 10000 and vessel.available_thrust > 0 and vessel.control.brakes == False: + vessel.control.brakes = True + + + # 2. 动态速度规划 (关键逻辑) + # 这是一个简化的着陆曲线：目标速度 = -sqrt(2 * 理想加速度 * 高度) + # 我们希望在靠近地面时速度减小到 -2 m/s + target_vel = -10000.0 # 高度较高时先全速下降 + if alt < 5000: + if alt > 500: + target_vel = -min(300.0, 0.5 * alt) # 高空较快下降 + elif alt > 200: + target_vel = -min(100.0, 0.5 * alt) # 高空较快下降 + elif alt > 30: + # 打开刹车 + if vessel.control.brakes == True: + vessel.control.brakes = False + target_vel = -20.0 # 中高度恒定下降速度 + else: + target_vel = -3.0 # 接近地面时恒定慢速 + + + # 3. 触地检测 + if alt < 1.0 and abs(vel) < 0.5: + vessel.control.throttle = 0 + vessel.control.sas = True + print("已着陆！") + break + + # 4. 自动部署脚架 + if alt < 500: + vessel.control.gear = True + + # 5. PID 计算 + dt = 0.1 + error = target_vel - vel + raw_throttle = speed_pid.update(error, dt) + + # --- 简易低延迟滤波 (EMA) --- + # 使用 0.4 的 alpha 值兼顾了响应速度和稳定性 + throttle = (alpha * raw_throttle) + ((1.0 - alpha) * last_throttle) + + # 限制并在执行前更新 last_throttle + throttle = max(0.0, min(1.0, throttle)) + last_throttle = throttle + + vessel.control.throttle = throttle + + print(f"高度: {alt:.2f}m | 目标速: {target_vel:.1f}m/s | 实际速: {vel:.1f}m/s | 节流阀: {throttle:.2f}") + time.sleep(dt) \ No newline at end of file diff --git a/测试文件/着陆v2.py b/测试文件/着陆v2.py new file mode 100644 index 0000000..dd7a16d --- /dev/null +++ b/测试文件/着陆v2.py @@ -0,0 +1,136 @@ +import time +import krpc +import math + +# ================= 配置参数 (V5 - 针对稳定性与精准度重置) ================= +TARGET_ALTITUDE = 50.0 + +# 垂直控制 (双环) +ALT_OUTER_KP = 0.3 # 适度增大外环增益，平衡响应速度与稳定性 +ALT_INNER_KP = 0.05 # 适度增大速度环增益 +ALT_INNER_KI = 0.03 # 增大积分项，消除稳态误差 +ALT_INNER_KD = 0.25 # 增大阻尼，抑制过冲 + +# 水平控制 (双环 - 极端提前减速 + 暴力制动版) +POS_KP = 0.04 # 目标速 = 距离 * 0.04 (375m处就开始限制速度，实现极早制动) +MAX_H_VEL = 15.0 + +VEL_KP = 0.12 # 增强力度，让姿态更果断地跟上速度变化 +VEL_KI = 0.01 +VEL_KD = 0.80 # 极强阻尼，防止因大角度刹车导致的过冲和摆动 +MAX_TILT = 0.50 # 允许最大倾角约 28 度！提供 SpaceX 级别的暴力水平制动力 + +DT = 0.1 # 采样周期 +ALPHA = 0.3 # 节流阀滤波 (越小越平滑) +# ========================================================================= + +conn = krpc.connect(name='Hover Guidance V5') +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, limit=1.0): + self.kp, self.ki, self.kd = kp, ki, kd + self.limit = limit + self.integral = 0 + self.prev_error = 0 + + def update(self, error, dt): + self.integral = max(-self.limit, min(self.limit, self.integral + error * dt)) + derivative = (error - self.prev_error) / dt + self.prev_error = error + return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative) + +# 初始化所有控制器 +alt_pid = PID(ALT_INNER_KP, ALT_INNER_KI, ALT_INNER_KD, limit=1.0) +vel_n_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1) +vel_e_pid = PID(VEL_KP, VEL_KI, VEL_KD, limit=0.1) + +# 初始化位置环积分 +pos_n_integral = 0.0 +pos_e_integral = 0.0 + +# # 给定目标点 (KSC 着陆台) +# target_lat = -0.097179090928 +# target_lon = -74.5576787510 + +# VBA房顶 +target_lat = -0.09684801697319373 +target_lon = -74.61749199803717 + + +body_radius = vessel.orbit.body.equatorial_radius +deg_to_m = (2 * math.pi * body_radius) / 360.0 + +# 准备状态 +vessel.auto_pilot.engage() +vessel.auto_pilot.reference_frame = vessel.surface_reference_frame +last_throttle = 0.0 + +print(f"制导系统 V5 已启动，目标高度: {TARGET_ALTITUDE}m") + +while True: + # 1. 采集速度数据 (使用 surface_velocity_reference_frame 获取地表相对速度) + v_surf = vessel.velocity(vessel.surface_velocity_reference_frame) + v_up = v_surf[0] + v_north = v_surf[1] + v_east = v_surf[2] + + flight = vessel.flight(vessel.surface_reference_frame) + alt = flight.surface_altitude + lat = flight.latitude + lon = flight.longitude + + # 2. 水平层逻辑 (制导) + # 计算位移误差（米） + err_n = (target_lat - lat) * deg_to_m + err_e = (target_lon - lon) * deg_to_m * math.cos(math.radians(lat)) + + # 调试信息打印 (提高精度到小数点后 6 位，显示微小速度变化) + dist = math.sqrt(err_n**2 + err_e**2) + + # 【核心重构】：线性减速模型。 + # 我们不再使用复杂的五段式，而是使用一个平滑的比例： + # 目标速度 = 距离 * 0.05。 + # 比如在 100m 处，目标速度就是 5m/s。 + # 在 20m 处，目标速度就是 1m/s。 + # 这样火箭从 300m 外就会开始感觉到“目标速度在变小”，从而实现极早的反向机动。 + target_vn = max(-MAX_H_VEL, min(MAX_H_VEL, err_n * POS_KP)) + target_ve = max(-MAX_H_VEL, min(MAX_H_VEL, err_e * POS_KP)) + + print(f"距离: {dist:.1f}m | 目标速: {math.sqrt(target_vn**2+target_ve**2):.1f} | N速: {v_north:.3f}") + + # 内环: 目标速 - 实速 -> 倾角 + tilt_n = vel_n_pid.update(target_vn - v_north, DT) + tilt_e = vel_e_pid.update(target_ve - v_east, DT) + + # 限制并设置姿态 + tilt_n = max(-MAX_TILT, min(MAX_TILT, tilt_n)) + tilt_e = max(-MAX_TILT, min(MAX_TILT, tilt_e)) + vessel.auto_pilot.target_direction = (1.0, tilt_n, tilt_e) + + # 3. 垂直层逻辑 + # 外环: 高度误差 -> 目标垂直速 (P) + target_vs = (TARGET_ALTITUDE - alt) * ALT_OUTER_KP + target_vs = max(-10.0, min(10.0, target_vs)) + + # 内环: 速度误差 -> 节流阀 (PID) + raw_throttle = alt_pid.update(target_vs - v_up, DT) + + # 加权补偿 (抵消因垂直向量偏置导致的升力损失) + cos_tilt = math.cos(math.sqrt(tilt_n**2 + tilt_e**2)) + throttle_corr = raw_throttle / max(0.8, cos_tilt) + + # 4. 滤波输出 + throttle = (ALPHA * throttle_corr) + ((1.0 - ALPHA) * last_throttle) + throttle = max(0.0, min(1.0, throttle)) + vessel.control.throttle = throttle + last_throttle = throttle + + # 5. 信息反馈 + dist = math.sqrt(err_n**2 + err_e**2) + print(f"距离: {dist:.2f}m | 高度: {alt:.1f}m | N_速: {v_north:.2f}m/s | 节流阀: {throttle:.2f}") + + time.sleep(DT) diff --git a/测试文件/采集当前坐标.py b/测试文件/采集当前坐标.py new file mode 100644 index 0000000..160cdfe --- /dev/null +++ b/测试文件/采集当前坐标.py @@ -0,0 +1,15 @@ +import krpc + + +conn = krpc.connect(name='Hover Guidance V5') +if not conn.space_center: + print("No active vessel found.") + exit() +vessel = conn.space_center.active_vessel + +flight = vessel.flight(vessel.surface_reference_frame) + +lat = flight.latitude +lon = flight.longitude + +print(f"当前位置：纬度 {lat}，经度 {lon}") \ No newline at end of file diff --git a/测试文件/采集当前速度.py b/测试文件/采集当前速度.py new file mode 100644 index 0000000..ec60fb7 --- /dev/null +++ b/测试文件/采集当前速度.py @@ -0,0 +1,12 @@ +import krpc + + +conn = krpc.connect(name='Hover Guidance V5') +if not conn.space_center: + print("No active vessel found.") + exit() +vessel = conn.space_center.active_vessel + +flight = vessel.flight(vessel.surface_reference_frame) +v_surf = vessel.velocity(vessel.orbital_reference_frame) +print(f"地表相对速度：上升 {v_surf[0]:.2f} m/s，北向 {v_surf[1]:.2f} m/s，东向 {v_surf[2]:.2f} m/s") diff --git a/自动着陆.py b/自动着陆.py new file mode 100644 index 0000000..a9d7356 --- /dev/null +++ b/自动着陆.py @@ -0,0 +1,90 @@ +from enum import Enum +import time +import krpc +from pid import PID +conn = krpc.connect(name='SpaceX Landing') +if not conn.space_center: + print("No active vessel found.") + exit() + +# control = vessel.control +vessel = conn.space_center.active_vessel +celestial = vessel.orbit.body # 当前天体 +# 创建 PID 控制器 +# 外环：高度 -> 目标垂直速度（允许负值） +altitude_pid = PID(kp=0.3, ki=0.03, kd=0.05, integral_limit=2.0, alpha=0.8, output_limit=(-10.0, 10.0)) + +# 内环：垂直速度 -> 节流阀（0~1） +speed_pid = PID(kp=0.06, ki=0.05, kd=0.05, integral_limit=10.0, alpha=0.5, output_limit=(0.0, 1.0)) + +target_altitude = 30 # 目标高度，单位 m + +# 状态机定义：高于 100m 为滑翔（GLIDE），100m 及以下为减速着陆（LAND） +class STATE(str, Enum): + GLIDE = "GLIDE" + LAND = "LAND" + +current_state = STATE.GLIDE + +last_time = time.monotonic() +loop_dt = 0.1 + +# 进入减速着陆时使用的目标垂直速度（保持原有行为） +target_vertical_speed = -3 + +try: + while True: + now = time.monotonic() + dt = now - last_time + last_time = now + + flight = vessel.flight(vessel.orbit.body.reference_frame) + current_altitude = flight.surface_altitude + current_vertical_speed = flight.vertical_speed + + # 计算转换距离 + available_thrust = vessel.available_thrust # 可用推力 + gav = celestial.surface_gravity # 重力加速度 + twr = available_thrust / (vessel.mass * gav) # 推重比 + if twr > 1.0: + transition_distance = (current_vertical_speed**2) / (2 * gav * (twr - 1)) # 米 + else: + transition_distance = float('inf') # 无法减速，设为无穷大 + + # 状态判断：大于 200m 为滑翔（无需控制，油门 0），否则为减速着陆（使用现有内环 PID） + if current_state == STATE.GLIDE: # 禁止反向切换 + new_state = STATE.GLIDE if current_altitude > transition_distance else STATE.LAND + + if new_state != current_state and new_state is not None: + print(f"状态切换: {current_state} -> {new_state}") + current_state = new_state + # 在状态切换时重置 PID，避免积分风up + try: + altitude_pid.reset() + except Exception: + pass + try: + speed_pid.reset() + except Exception: + pass + + if current_state == STATE.GLIDE: + # 滑翔：不做具体控制，油门设为 0 + vessel.control.throttle = 0.0 + print(f"当前高度: {current_altitude:.2f} m, 预计点火距离: {transition_distance:.2f} m, 状态: GLIDE, 油门: 0.00"," "*10, end='\r') + else: + # 内环：垂直速度误差 -> 节流阀 + speed_error = target_vertical_speed - current_vertical_speed + throttle = speed_pid.update(speed_error, dt) + + vessel.control.throttle = throttle + print(f"当前高度: {current_altitude:.2f} m, 预计点火距离: {transition_distance:.2f} m, 目标速: {target_vertical_speed:.2f} m/s, 速: {current_vertical_speed:.2f} m/s, 节流阀: {throttle:.2f}"," "*10, end='\r') + + # 保持大致的控制周期 + sleep_time = loop_dt - (time.monotonic() - now) + if sleep_time > 0: + time.sleep(sleep_time) + +except KeyboardInterrupt: + vessel.control.throttle = 0.0 + print("已停止控制，节流设为 0。") \ No newline at end of file