feat: 增强着陆控制逻辑，加入水平位置修正和状态切换机制

自动着陆.py

@@ -7,8 +7,14 @@ from pid import PID
 class LandingState(str, Enum):
     GLIDE = "GLIDE"      # 滑翔/自由落体
     LAND = "LAND"        # 动态减速
+    FIXED = "FIXED"      # 水平位置修正
     FINAL = "FINAL"      # 10m以下匀速着陆
 
+# 给定目标点 (KSC 着陆台)
+TARGET_LAT = -0.097179090928
+TARGET_LON = -74.5576787510
+
+
 class SpaceX_Lander:
     def __init__(self):
         # 初始化连接
@@ -21,21 +27,24 @@ class SpaceX_Lander:
         self.celestial = self.vessel.orbit.body
         
         # 控制参数
-        self.target_landing_speed = -1.0  # m/s
+        self.target_landing_speed = -1.5  # m/s
-        self.final_approach_alt = 10.0    # m
+        self.final_approach_alt = 30.0    # m
         self.safety_factor = 0.8          # 动力冗余
         self.loop_dt = 0.05               # 控制周期
         
         # PID 控制器
-        # 垂直速度 -> 节流阀 (不需要变，保持现状)
+        # 垂直速度 -> 节流阀 (维持悬停稳高度)
-        self.vertical_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))
+        self.vertical_speed_pid = PID(kp=0.15, ki=0.1, kd=0.05, integral_limit=10.0, alpha=0.5, output_limit=(0.0, 1.0))
         
-        # 横向速度 -> 倾斜角度 (Aggressive Tuning)
+        # 位置控制 PID (外环)：距离距离误差 -> 目标速度
-        # kp 增加到 3.0：意指每 1m/s 的横向速度误差，就倾斜 3 度去修正
+        # kp 减小以平滑接近，kd 增加提供提前减速，限制最大速度为 5m/s 保证精确度
-        # ki 增加到 0.02：消除残余稳态误差
+        self.pos_north_pid = PID(kp=0.15, ki=0.01, kd=0.4, integral_limit=5.0, alpha=0.5, output_limit=(-30.0, 30.0))
-        # kd 增加到 0.1：提供更好的阻尼，防止震荡
+        self.pos_east_pid = PID(kp=0.15, ki=0.01, kd=0.4, integral_limit=5.0, alpha=0.5, output_limit=(-30.0, 30.0))
-        self.horizontal_north_pid = PID(kp=3.0, ki=0.02, kd=0.1, integral_limit=15.0, alpha=0.5, output_limit=(-20.0, 20.0))
+        
-        self.horizontal_east_pid = PID(kp=3.0, ki=0.02, kd=0.1, integral_limit=15.0, alpha=0.5, output_limit=(-20.0, 20.0))
+        # 速度控制 PID (内环)：速度误差 -> 倾斜角度
+        # 增加 kp 提高跟随性，大幅增加 kd (0.8 -> 2.0) 强化刹车阻尼
+        self.horizontal_north_pid = PID(kp=4.0, ki=0.02, kd=2.0, integral_limit=15.0, alpha=0.5, output_limit=(-20.0, 20.0))
+        self.horizontal_east_pid = PID(kp=4.0, ki=0.02, kd=2.0, integral_limit=15.0, alpha=0.5, output_limit=(-20.0, 20.0))
         
         # 状态
         self.state = LandingState.GLIDE
@@ -87,6 +96,31 @@ class SpaceX_Lander:
                 current_vertical_speed = flight.vertical_speed
                 h_err = max(0.0, current_altitude - self.bottom_offset)
 
+
+                lat = flight.latitude # 当前位置纬度
+                lon = flight.longitude # 当前位置经度
+                # 转换经纬度偏差为米 (近似计算：1度纬度 ≈ 111319m 在 Kerbin)
+                # 纬度 1度对应的米数
+                lat_to_m = (self.celestial.equatorial_radius * math.pi) / 180.0
+                # 经度 1度对应的米数 (随纬度变化)
+                lon_to_m = lat_to_m * math.cos(math.radians(lat))
+                
+                north_dist_error = (TARGET_LAT - lat) * lat_to_m
+                east_dist_error = (TARGET_LON - lon) * lon_to_m
+                
+                horizontal_error = math.sqrt(north_dist_error**2 + east_dist_error**2)
+
+                # 外环：根据位置偏差计算“目标速度”
+                target_v_north = self.pos_north_pid.update(north_dist_error, dt)
+                target_v_east = self.pos_east_pid.update(east_dist_error, dt)
+
+                # 内环：根据速度偏差 (目标速度 - 当前速度) 计算“目标倾角”
+                north_error = target_v_north - current_velocity[1]
+                east_error = target_v_east - current_velocity[2]
+                
+                north_output = self.horizontal_north_pid.update(north_error, dt)
+                east_output = self.horizontal_east_pid.update(east_error, dt)
+
                 # 计算环境与物理参数
                 available_thrust = self.vessel.available_thrust
                 gav = self.celestial.surface_gravity
@@ -108,6 +142,16 @@ class SpaceX_Lander:
                 
                 elif self.state == LandingState.LAND:
                     if h_err <= self.final_approach_alt:
+                        # 如果水平误差过大，先进入悬停修正模式
+                        if horizontal_error > 5.0:
+                            new_state = LandingState.FIXED
+                        else:
+                            new_state = LandingState.FINAL
+                
+                elif self.state == LandingState.FIXED:
+                    # 当水平误差 < 3m 且水平速度 < 0.5m/s 时，继续着陆
+                    h_vel_mag = math.sqrt(current_velocity[1]**2 + current_velocity[2]**2)
+                    if horizontal_error < 3.0 and h_vel_mag < 0.5:
                         new_state = LandingState.FINAL
 
                 if new_state != self.state:
@@ -115,50 +159,53 @@ class SpaceX_Lander:
 
                 # 2. 状态执行逻辑
                 throttle = 0.0
-                
+                # 动力阶段（LAND/FINAL）需要倾斜推力来反推；气动阶段（GLIDE）则需要调整攻角
-                # 计算横向控制输出 (全阶段开启，除非触地瞬间)
+                # 适当增加低空倾斜限制，允许更强的刹车能力
-                # 目标横向速度为 0
+                dynamic_max_tilt = max(10.0, min(25.0, h_err * 0.5))
-                north_error = 0 - current_velocity[1]
-                east_error = 0 - current_velocity[2]
-                
-                north_output = self.horizontal_north_pid.update(north_error, dt)
-                east_output = self.horizontal_east_pid.update(east_error, dt)
-
-                # 根据不同阶段应用差异化的横向姿态控制
-                # 动力阶段（LAND/FINAL）需要倾斜推力来反推；气动阶段（GLIDE）则需要调整攻角利用阻力和升力
-
-
-
-                dynamic_max_tilt = max(5.0, min(20.0, h_err * 0.4))
                 
                 if self.state == LandingState.GLIDE:
                     # 气动阶段：采用反向纠偏策略 (依靠舰体气动效应)
-                    self.set_steering_with_lateral_error(-east_output * 3, -north_output * 3, max_tilt=dynamic_max_tilt)
+                    self.set_steering_with_lateral_error(-east_output * 4, -north_output * 4, max_tilt=dynamic_max_tilt)
                 else:
                     # 动力阶段：采用正向纠偏策略 (依靠引擎推力矢量)
                     self.set_steering_with_lateral_error(east_output, north_output, max_tilt=dynamic_max_tilt)
 
+                # 获取当前相对于垂向的倾斜角，用于推力补偿
+                pitch = flight.pitch
+                tilt_rad = math.radians(abs(90.0 - pitch))
+                
                 if self.state == LandingState.GLIDE:
                     throttle = 0.0
-                    print(f"高度: {h_err:.1f}m, 点火距: {transition_distance:.1f}m, 状态: GLIDE, 北速: {current_velocity[1]:.1f}m/s, 东速: {current_velocity[2]:.1f}m/s", " "*5, end="\r")
+                    print(f"高度: {h_err:.1f}m, 点火距: {transition_distance:.1f}m, 状态: GLIDE, 北速: {current_velocity[1]:.1f}m/s, 东速: {current_velocity[2]:.1f}m/s, 水平误差: {horizontal_error:.1f}m", " "*5, end="\r")
                 
                 elif self.state == LandingState.LAND:
                     # 动态减速曲线
                     a_plan = max(0.1, a_net_max * self.safety_factor)
                     ideal_speed = -(max(0, self.target_landing_speed**2 + 2 * a_plan * (h_err - self.final_approach_alt))**0.5)
-                    throttle = self.vertical_speed_pid.update(ideal_speed - current_vertical_speed, dt)
+                    raw_throttle = self.vertical_speed_pid.update(ideal_speed - current_vertical_speed, dt)
+                    # 推力倾斜补偿：throttle_final = throttle_raw / cos(tilt)
+                    throttle = raw_throttle / math.cos(tilt_rad) if math.cos(tilt_rad) > 0.5 else raw_throttle
                     
-                    print(f"高度: {h_err:.1f}m, 理想速: {ideal_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: LAND, 偏角: {dynamic_max_tilt:.1f}", " "*5, end="\r")
+                    print(f"高度: {h_err:.1f}m, 状态: LAND, 距目标: {horizontal_error:.1f}m, 目标速: {math.sqrt(target_v_north**2 + target_v_east**2):.1f}m/s", " "*5, end="\r")
                 
+                elif self.state == LandingState.FIXED:
+                    # 水平误差修正阶段：悬停并等待水平误差减慢
+                    raw_throttle = self.vertical_speed_pid.update(0.0 - current_vertical_speed, dt)
+                    throttle = raw_throttle / math.cos(tilt_rad) if math.cos(tilt_rad) > 0.5 else raw_throttle
+                    h_vel_mag = math.sqrt(current_velocity[1]**2 + current_velocity[2]**2)
+                    print(f"高度: {h_err:.1f}m, 状态: FIXED, 水平偏差: {horizontal_error:.2f}m, 水平速: {h_vel_mag:.2f}m/s", " "*5, end="\r")
+
                 elif self.state == LandingState.FINAL:
                     # 匀速下降阶段
-                    throttle = self.vertical_speed_pid.update(self.target_landing_speed - current_vertical_speed, dt)
+                    target_landing_speed = max(- (h_err / 3.0), -1.5)
-                    print(f"高度: {h_err:.1f}m, 目标速: {self.target_landing_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: FINAL", " "*5, end="\r")
+                    raw_throttle = self.vertical_speed_pid.update(target_landing_speed - current_vertical_speed, dt)
+                    throttle = raw_throttle / math.cos(tilt_rad) if math.cos(tilt_rad) > 0.5 else raw_throttle
+                    print(f"高度: {h_err:.1f}m, 目标速: {target_landing_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: FINAL", " "*5, end="\r")
                     
                     # 触地检测
                     if h_err < 0.5 and abs(current_vertical_speed) < 0.5:
                         self.vessel.control.throttle = 0.0
-                        print("\n触地成功，任务完成。")
+                        print(f"\n触地成功，任务完成。水平误差: {horizontal_error:.3f}m")
                         break
 
                 self.vessel.control.throttle = throttle
@@ -176,6 +223,20 @@ class SpaceX_Lander:
         print(f"\n[状态切换]: {self.state} -> {new_state}")
         self.state = new_state
         self.vertical_speed_pid.reset()
+        
+        # 根据不同阶段动态调整 PID 灵敏度
+        if new_state == LandingState.FIXED:
+            # 精确悬停模式：低速、高阻尼
+            self.pos_north_pid.output_limit = (-5.0, 5.0)
+            self.pos_east_pid.output_limit = (-5.0, 5.0)
+            self.horizontal_north_pid.kd = 2.0
+            self.horizontal_east_pid.kd = 2.0
+        else:
+            # 飞行与减速模式：高速追赶目标，中等阻尼
+            self.pos_north_pid.output_limit = (-50.0, 50.0)
+            self.pos_east_pid.output_limit = (-50.0, 50.0)
+            self.horizontal_north_pid.kd = 0.5
+            self.horizontal_east_pid.kd = 0.5
 
 if __name__ == "__main__":
     lander = SpaceX_Lander()