feat: 初始化提交

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