kaku/krpc-auto-landing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
kakune55
2026-01-31 13:40:05 +08:00
parent 4b4736ecce
commit a9f6b25581
1 changed files with 93 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
自动着陆.py
View File

@@ -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.final_approach_alt = 10.0 # m
self.target_landing_speed = -1.5 # m/s
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)
# kp 增加到 3.0：意指每 1m/s 的横向速度误差，就倾斜 3 度去修正
# ki 增加到 0.02：消除残余稳态误差
# kd 增加到 0.1：提供更好的阻尼，防止震荡
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 增加提供提前减速，限制最大速度为 5m/s 保证精确度
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))
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))
# 速度控制 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
# 计算横向控制输出 (全阶段开启，除非触地瞬间)
# 目标横向速度为 0
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))
# 动力阶段（LAND/FINAL）需要倾斜推力来反推；气动阶段（GLIDE）则需要调整攻角
# 适当增加低空倾斜限制，允许更强的刹车能力
dynamic_max_tilt = max(10.0, min(25.0, h_err * 0.5))
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)
print(f"高度: {h_err:.1f}m, 目标速: {self.target_landing_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: FINAL", " "*5, end="\r")
target_landing_speed = max(- (h_err / 3.0), -1.5)
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()