137 lines
4.9 KiB
Python
137 lines
4.9 KiB
Python
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)
|