feat: 优化着陆控制逻辑，增强状态机定义和高度修正

2026-01-31 11:54:05 +08:00
parent e251d681ab
commit 7d54dd5eb3
1 changed files with 66 additions and 24 deletions
--- a/自动着陆.py
+++ b/自动着陆.py
@@ -19,20 +19,32 @@ speed_pid = PID(kp=0.06, ki=0.05, kd=0.05, integral_limit=10.0, alpha=0.5, outpu

 target_altitude = 30  # 目标高度，单位 m

-# 状态机定义：高于 100m 为滑翔（GLIDE），100m 及以下为减速着陆（LAND）
+# 状态机定义
 class STATE(str, Enum):
-    GLIDE = "GLIDE"
-    LAND = "LAND"
+    GLIDE = "GLIDE"      # 滑翔/自由落体
+    LAND = "LAND"        # 动态减速
+    FINAL = "FINAL"      # 10m以下匀速着陆

 current_state = STATE.GLIDE

 last_time = time.monotonic()
-loop_dt = 0.1
+loop_dt = 0.1 # 降低周期提高采样频率

-# 进入减速着陆时使用的目标垂直速度（保持原有行为）
-target_vertical_speed = -3
+# 着陆控制参数
+target_landing_speed = -1.0  # 最终落地目标垂直速度 (m/s)
+final_approach_alt = 10.0    # 切换到匀速下降的高度 (m)
+safety_factor = 0.8          # 冗余因素

 try:
+    # 自动获取飞船高度偏移 (从质心到底部的距离)
+    # 在 vessel 坐标系中，通常 y 轴是纵向的
+    try:
+        box = vessel.bounding_box(vessel.reference_frame)
+        bottom_offset = abs(box[0][1]) 
+    except:
+        bottom_offset = 0.0
+    print(f"检测到飞船底部偏移量: {bottom_offset:.2f} m")
+
    while True:
        now = time.monotonic()
        dt = now - last_time
@@ -42,43 +54,73 @@ try:
        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)) # 米
+        a_max = available_thrust / vessel.mass
+        a_net_max = a_max - gav # 扣除重力后的最大净加速度
+        
+        # 修正高度：减掉底部偏移量 (h_err 是底部到地面的距离)
+        h_err = max(0.0, current_altitude - bottom_offset)
+
+        # 核心算法优化：计算点火距离时加入 safety_factor 提早点火
+        if a_net_max > 0.5:
+            # 预留更多余量 (safety_factor 越小，预估点火高度越高)
+            a_plan = max(0.1, a_net_max * safety_factor)
+            transition_distance = (current_vertical_speed**2 - target_landing_speed**2) / (2 * a_plan)
        else:
-            transition_distance = float('inf')  # 无法减速，设为无穷大
+            transition_distance = 1e6

-        # 状态判断：大于 200m 为滑翔（无需控制，油门 0），否则为减速着陆（使用现有内环 PID）
-        if current_state == STATE.GLIDE: # 禁止反向切换
-            new_state = STATE.GLIDE if current_altitude > transition_distance else STATE.LAND
+        # 状态切换逻辑
+        new_state = current_state
+        if current_state == STATE.GLIDE: 
+            if h_err <= transition_distance:
+                new_state = STATE.LAND
+        elif current_state == STATE.LAND:
+            if h_err <= final_approach_alt:
+                new_state = STATE.FINAL

-        if new_state != current_state and new_state is not None:
-            print(f"状态切换: {current_state} -> {new_state}")
+        if new_state != current_state:
+            print(f"\n状态切换: {current_state} -> {new_state}")
            current_state = new_state
            # 在状态切换时重置 PID，避免积分风up
            try:
                altitude_pid.reset()
-            except Exception:
-                pass
-            try:
                speed_pid.reset()
-            except Exception:
+            except:
                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:
+            print(f"高度: {h_err:.2f} m, 预计点火点: {transition_distance:.2f} m, 状态: GLIDE, 油门: 0.00"," "*10, end='\r')
+        elif current_state == STATE.LAND:
+            # 动态轨迹：根据当前高度计算理想减速梯度
+            a_plan = max(0.1, a_net_max * safety_factor)
+            # 目标速度公式: 随高度平滑减速
+            ideal_speed = -( max(0, target_landing_speed**2 + 2 * a_plan * (h_err - final_approach_alt))**0.5 )
+            
            # 内环：垂直速度误差 -> 节流阀
-            speed_error = target_vertical_speed - current_vertical_speed
+            speed_error = ideal_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')
+            print(f"高度: {h_err:.2f} m, 理想速: {ideal_speed:.2f} m/s, 当前速: {current_vertical_speed:.2f} m/s, 状态: LAND, 节流阀: {throttle:.2f}"," "*10, end='\r')
+        
+        elif current_state == STATE.FINAL:
+            # 10m 以下：匀速缓慢下降
+            target_v = target_landing_speed
+            speed_error = target_v - current_vertical_speed
+            throttle = speed_pid.update(speed_error, dt)
+            
+            vessel.control.throttle = throttle
+            # 到达极低高度或检测到接触（垂直速度接近0）时关闭引擎
+            if h_err < 0.5 and abs(current_vertical_speed) < 0.5:
+                vessel.control.throttle = 0.0
+                print("\n检测到触地，引擎关闭。")
+                break
+            
+            print(f"高度: {h_err:.2f} m, 目标速: {target_v:.2f} m/s, 当前速: {current_vertical_speed:.2f} m/s, 状态: FINAL, 节流阀: {throttle:.2f}"," "*10, end='\r')

        # 保持大致的控制周期
        sleep_time = loop_dt - (time.monotonic() - now)