feat: 重构着陆控制逻辑，优化状态机和PID控制器实现
This commit is contained in:
254
自动着陆.py
254
自动着陆.py
@@ -1,132 +1,148 @@
|
||||
from enum import Enum
|
||||
import time
|
||||
import math
|
||||
import krpc
|
||||
from pid import PID
|
||||
conn = krpc.connect(name='SpaceX Landing')
|
||||
if not conn.space_center:
|
||||
print("No active vessel found.")
|
||||
exit()
|
||||
|
||||
# control = vessel.control
|
||||
vessel = conn.space_center.active_vessel
|
||||
celestial = vessel.orbit.body # 当前天体
|
||||
# 创建 PID 控制器
|
||||
# 外环：高度 -> 目标垂直速度（允许负值）
|
||||
altitude_pid = PID(kp=0.3, ki=0.03, kd=0.05, integral_limit=2.0, alpha=0.8, output_limit=(-10.0, 10.0))
|
||||
|
||||
# 内环：垂直速度 -> 节流阀（0~1）
|
||||
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))
|
||||
|
||||
target_altitude = 30 # 目标高度，单位 m
|
||||
|
||||
# 状态机定义
|
||||
class STATE(str, Enum):
|
||||
class LandingState(str, Enum):
|
||||
GLIDE = "GLIDE" # 滑翔/自由落体
|
||||
LAND = "LAND" # 动态减速
|
||||
FINAL = "FINAL" # 10m以下匀速着陆
|
||||
|
||||
current_state = STATE.GLIDE
|
||||
|
||||
last_time = time.monotonic()
|
||||
loop_dt = 0.1 # 降低周期提高采样频率
|
||||
|
||||
# 着陆控制参数
|
||||
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
|
||||
last_time = now
|
||||
|
||||
flight = vessel.flight(vessel.orbit.body.reference_frame)
|
||||
current_altitude = flight.surface_altitude
|
||||
current_vertical_speed = flight.vertical_speed
|
||||
|
||||
# 计算转换距离与高度修正
|
||||
available_thrust = vessel.available_thrust # 可用推力
|
||||
gav = celestial.surface_gravity # 重力加速度
|
||||
a_max = available_thrust / vessel.mass
|
||||
a_net_max = a_max - gav # 扣除重力后的最大净加速度
|
||||
class SpaceX_Lander:
|
||||
def __init__(self):
|
||||
# 初始化连接
|
||||
self.conn = krpc.connect(name="SpaceX Landing Controller")
|
||||
if not self.conn.space_center:
|
||||
print("No active vessel found.")
|
||||
exit()
|
||||
|
||||
self.vessel = self.conn.space_center.active_vessel
|
||||
self.celestial = self.vessel.orbit.body
|
||||
|
||||
# 修正高度：减掉底部偏移量 (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 = 1e6
|
||||
|
||||
# 状态切换逻辑
|
||||
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:
|
||||
print(f"\n状态切换: {current_state} -> {new_state}")
|
||||
current_state = new_state
|
||||
# 在状态切换时重置 PID，避免积分风up
|
||||
try:
|
||||
altitude_pid.reset()
|
||||
speed_pid.reset()
|
||||
except:
|
||||
pass
|
||||
|
||||
if current_state == STATE.GLIDE:
|
||||
# 滑翔：不做具体控制，油门设为 0
|
||||
vessel.control.throttle = 0.0
|
||||
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 = ideal_speed - current_vertical_speed
|
||||
throttle = speed_pid.update(speed_error, dt)
|
||||
|
||||
vessel.control.throttle = throttle
|
||||
print(f"高度: {h_err:.2f} m, 理想速: {ideal_speed:.2f} m/s, 当前速: {current_vertical_speed:.2f} m/s, 状态: LAND, 节流阀: {throttle:.2f}"," "*10, end='\r')
|
||||
# 控制参数
|
||||
self.target_landing_speed = -1.0 # m/s
|
||||
self.final_approach_alt = 10.0 # m
|
||||
self.safety_factor = 0.8 # 动力冗余
|
||||
self.loop_dt = 0.05 # 控制周期
|
||||
|
||||
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')
|
||||
# PID 控制器
|
||||
self.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.state = LandingState.GLIDE
|
||||
self.bottom_offset = self._calculate_bottom_offset()
|
||||
|
||||
print(f"SpaceX Lander 准备就绪。底部偏移: {self.bottom_offset:.2f}m")
|
||||
|
||||
# 保持大致的控制周期
|
||||
sleep_time = loop_dt - (time.monotonic() - now)
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
def _calculate_bottom_offset(self):
|
||||
"""自动计算飞船质心到底部的距离"""
|
||||
try:
|
||||
box = self.vessel.bounding_box(self.vessel.reference_frame)
|
||||
return abs(box[0][1])
|
||||
except:
|
||||
return 0.0
|
||||
|
||||
def set_steering_with_lateral_error(self, east_err, north_err, max_tilt=20.0):
|
||||
"""将横向误差转化为姿态"""
|
||||
tilt = math.sqrt(east_err**2 + north_err**2)
|
||||
if tilt > max_tilt:
|
||||
scale = max_tilt / tilt
|
||||
east_err *= scale
|
||||
north_err *= scale
|
||||
tilt = max_tilt
|
||||
|
||||
target_azimuth = math.degrees(math.atan2(east_err, north_err))
|
||||
target_pitch = 90.0 - tilt
|
||||
|
||||
self.vessel.auto_pilot.target_pitch_and_heading(target_pitch, target_azimuth)
|
||||
self.vessel.auto_pilot.engage()
|
||||
|
||||
def run(self):
|
||||
"""主运行循环"""
|
||||
last_time = time.monotonic()
|
||||
|
||||
try:
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
dt = now - last_time
|
||||
last_time = now
|
||||
|
||||
# 获取实时飞行数据
|
||||
flight = self.vessel.flight(self.celestial.reference_frame)
|
||||
current_altitude = flight.surface_altitude
|
||||
current_velocity = flight.velocity
|
||||
current_vertical_speed = flight.vertical_speed
|
||||
h_err = max(0.0, current_altitude - self.bottom_offset)
|
||||
|
||||
# 计算环境与物理参数
|
||||
available_thrust = self.vessel.available_thrust
|
||||
gav = self.celestial.surface_gravity
|
||||
a_max = available_thrust / self.vessel.mass if self.vessel.mass > 0 else 0
|
||||
a_net_max = a_max - gav
|
||||
|
||||
# 1. 状态切换逻辑与点火计算
|
||||
new_state = self.state
|
||||
transition_distance = 0
|
||||
if self.state == LandingState.GLIDE:
|
||||
if a_net_max > 0.5:
|
||||
a_plan = max(0.1, a_net_max * self.safety_factor)
|
||||
transition_distance = (current_vertical_speed**2 - self.target_landing_speed**2) / (2 * a_plan)
|
||||
else:
|
||||
transition_distance = 1e6
|
||||
|
||||
if h_err <= transition_distance:
|
||||
new_state = LandingState.LAND
|
||||
|
||||
elif self.state == LandingState.LAND:
|
||||
if h_err <= self.final_approach_alt:
|
||||
new_state = LandingState.FINAL
|
||||
|
||||
if new_state != self.state:
|
||||
self._switch_state(new_state)
|
||||
|
||||
# 2. 状态执行逻辑
|
||||
throttle = 0.0
|
||||
|
||||
if self.state == LandingState.GLIDE:
|
||||
throttle = 0.0
|
||||
print(f"高度: {h_err:.1f}m, 点火距: {transition_distance:.1f}m, 东速: {current_velocity[1]:.1f}m/s, 北速: {current_velocity[0]:.1f}m/s, 状态: GLIDE", " "*10, 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.speed_pid.update(ideal_speed - current_vertical_speed, dt)
|
||||
print(f"高度: {h_err:.1f}m, 理想速: {ideal_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: LAND", " "*10, end="\r")
|
||||
|
||||
elif self.state == LandingState.FINAL:
|
||||
# 匀速下降阶段
|
||||
throttle = self.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", " "*10, end="\r")
|
||||
|
||||
# 触地检测
|
||||
if h_err < 0.5 and abs(current_vertical_speed) < 0.5:
|
||||
self.vessel.control.throttle = 0.0
|
||||
print("\n触地成功，任务完成。")
|
||||
break
|
||||
|
||||
self.vessel.control.throttle = throttle
|
||||
|
||||
# 频率控制
|
||||
sleep_time = self.loop_dt - (time.monotonic() - now)
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
self.vessel.control.throttle = 0.0
|
||||
print("\n程序手动停止，油门已归零。")
|
||||
|
||||
def _switch_state(self, new_state):
|
||||
print(f"\n[状态切换]: {self.state} -> {new_state}")
|
||||
self.state = new_state
|
||||
self.speed_pid.reset()
|
||||
|
||||
if __name__ == "__main__":
|
||||
lander = SpaceX_Lander()
|
||||
lander.run()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
vessel.control.throttle = 0.0
|
||||
print("已停止控制，节流设为 0。")
|
||||
Reference in New Issue
Block a user