feat: 初始化提交

测试文件/动态着陆.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)