当前位置：首页 > news >正文

基于Basilisk库实现三种姿态的切换

news 2025/9/1 15:10:59

一、实际意义

在轨航天器的姿态模式切换是任务管理与资源约束协调的核心机制，其意义体现在多目标性能最优化与系统安全性的统一。典型任务周期往往要求在测地指向、日定向发电、目标追踪、测控通信等互斥或耦合的姿态需求间动态切换；通过基于状态机的模式管理与明确的时间调度，可在保证有效载荷成像、链路可用度与姿态稳定性的同时，满足能量、热控与动量管理约束。从控制轮观点看，模式切换引入了分段时变系统与混杂动态特性，切换逻辑直接影响闭环稳定性与鲁棒性；合理设计可避免在边界条件（如地影边缘、目标进出视场、反作用轮接近饱和）处频繁抖动与过冲。工程上，模式间的过渡轨迹（渐变权重或限幅律）与执行时序（导航更新->制导写入->控制计算->执行映射）需严格保证因果性，以消除“读未写”和竞态风险，并在地影段识别与能量低阈值场景下自动降级至日定向或日安全模式，实现任务与能量安全的动态权衡。面向系统级优化，模式切换还与动量卸载、磁控协同、地面站可见性预测等共同构成事件驱动调度问题，其目标是在长期运行中最大化任务收益并最小化执行成本。

Hill指向（对地/轨道相关指向）：常用于对地观测与成像几何约束控制，使载荷视轴随轨道径向或轨道坐标系稳定，便于条带拼接、相位中心标定与复访一致性；在近地轨道还能降低气动与重力梯度扰动耦合，提高姿态稳定与能耗效率。任务层面用于常规测绘、海洋与陆地监测、编队相对姿态基准等。

sun指向（对日与安全模式）：在能量与热控为主的阶段（如部署、日食出入、长时间阴影后恢复）使机体或太阳电池阵面对准太阳，最大化发电并维持热平衡；作为异常或资源紧张时的“安全模式”，通过低增益、轮速限幅与粗敏感器闭环，保证姿态可控与功率正值，等待进一步诊断或重构。

Ground指向（对地面站或地物）：在测控与数传时将高增益天线、激光通信终端或载荷视轴精确对准地面站或目标地点，提升链路预算与数据下传速率；也用于目标区域的定点凝视与跨平台协同观测。该模式需满足可见性、仰角与遮挡约束，并与轨道/能源窗口、动量卸载策略协同调度。

二、Basilisk框架

一般在实际情况下，对于航天器采用哪种模式，通常需要考虑地影、目标地面站位置等相关信息，这里首先为了弄清楚如何使用basilisk框架完成任务的切换，这里每30分钟选择一种模式，按照hill,sunsafe,ground模式切换，一共仿真6个小时。这里我们不再采用单Task形式进行，而是通过多Process和多Task的形式。将Dynamic动力学与Fswalgorithm算法两个进程，Dynamic动力学包括DynTask，Fswalgorithm算法包括HillTask，SunTask，LocTask，CtrlTask。这样做的好处是能够通过独立的Task，在进行某项定向算法的时候，其余Task不会执行，节约内存。

同时注意到三种算法最终都会反馈到Mrpfeedback，即接收制导信息，这里采用Basilisk的C封装网关消息的能力，通过AttGuidMsg_C_addAuthor这个函数，将多个制导链共同写入一条C网关信息，下游的控制器只需订阅一次网关即可。整体的仿真框架图如图所示。

三、仿真结果

现在对仿真结果进行验证，即看每次姿态控制是否达到了期望的姿态。首先需要将三种模式的期望姿态的验证方案进行一个数学建模。例如Hill指向，只需要验证x轴与径向单位向量同向即可

$\hat e_r = \frac{r_{N}}{\|r_{N}\|}$

现在只需要把本体系x轴在惯性系中的分量求出来，关键是先把MRP转成方向余弦矩阵，再把机体系单位轴向量映射到惯性系。

$C_{BN}(\sigma_{BN})=\frac{(1-\sigma_{BN}^T\sigma_{BN})I_3+2[\sigma_{BN}]_\times+2\,\sigma_{BN}\sigma_{BN}^T}{1+\sigma_{BN}^T\sigma_{BN}},\qquad a_{1,N}=C_{BN}^T\,e_1.$

$[\sigma_{BN}]_\times = \begin{bmatrix} 0 & -\sigma_3 & \sigma_2 \\ \sigma_3 & 0 & -\sigma_1 \\ -\sigma_2 & \sigma_1 & 0 \end{bmatrix}.$

对于Sun指向的验证，则需要验证太阳向量 $\hat v_B$ 与期望机体指向 $\hat s_B$ 的夹角作为指标，同时注意仅在非地影状态下计算指标

$\hat v_B=\frac{v_B}{\|v_B\|},\qquad \hat s_B=\frac{s_{\mathrm{cmd}}}{\|s_{\mathrm{cmd}}\|},\qquad$

$J_{\mathrm{Sun}}=1-\hat v_B^\top \hat s_B={1-\cos\theta_{\mathrm{Sun}}}$

对于loc指向的验证指标是“机体期望对齐轴在惯性系中的方向”与“地面站视线”（地面站到航天器的连线）”之间的夹角。只在可见性满足时计算。

$a_N = C_{NB}\,p_B,\qquad \hat\ell = \frac{r_{LN,N}-r_{BN,N}}{\|r_{LN,N}-r_{BN,N}\|},\qquad \theta_{\mathrm{Loc}}=\arccos\!\Big(\frac{a_N\cdot \hat\ell}{\|a_N\|}\Big).$

$J_{\mathrm{Loc}} = 1 - \frac{a_N}{\|a_N\|}\cdot \hat\ell.$

现在对仿真结果进行分析，仿真结果图如图所示

观察以上可以发现，在对太阳定向模式情况下，在非地影阶段一般能够有效控制，但如果在地影阶段开启对太阳指向时，则对齐效果则不是很好，而对于其他指向条件下，基本上都是能够有效完成姿态定向的。因此，对于单个航天器来说，合理地选择姿态是其任务周期内一个重要研究内容。通常情况下，大部分的时间都是处于对地定向操作，在一般过顶某地面站时会切换到对地面目标定向，当处于低电量的情况下且处于非地影阶段时通常采用对太阳定向操作。

四、源码

"""
Unified attitude mode switching demo (Hill/Nadir → SunSafe → GroundLocation),
switching every 30 minutes over 6 hours using Basilisk.Guidance chain:
- Hill/Nadir: hillPoint.attRefOutMsg → attTrackingError → AttGuid (gateway)
- SunSafe:    sunSafePoint.attGuidanceOutMsg → AttGuid (gateway)
- Ground:     locationPointing.attRefOutMsg → attTrackingError → AttGuid (gateway)mrpFeedback subscribes to the AttGuid gateway; mode switching is done by enabling
only one FSW task at a time (the task that writes to the gateway message).
"""import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.utilities import (SimulationBaseClass, macros as mc, unitTestSupport,orbitalMotion, simIncludeGravBody, simIncludeRW,RigidBodyKinematics as rbk)
from Basilisk.architecture import messaging
from Basilisk.simulation import (spacecraft, simpleNav, reactionWheelStateEffector,eclipse, coarseSunSensor, ephemerisConverter,groundLocation)
from Basilisk.fswAlgorithms import (hillPoint, attTrackingError, mrpFeedback,rwMotorTorque, cssWlsEst, sunSafePoint,locationPointing)def run_mode_switch(sim_hours: float = 6.0, segment_min: float = 30.0):# 1) Sim and processessim = SimulationBaseClass.SimBaseClass()dynProc = sim.CreateNewProcess("DynProc",priority=20)fswProc = sim.CreateNewProcess("FSWProc",priority=10)dt = mc.sec2nano(1.0)dynProc.addTask(sim.CreateNewTask("DynTask", dt))# 3 FSW tasks, only one enabled at a time + 1 controller task (always on)hillTask = sim.CreateNewTask("HillTask", dt)sunTask = sim.CreateNewTask("SunTask", dt)locTask = sim.CreateNewTask("LocTask", dt)ctrlTask = sim.CreateNewTask("CtrlTask", dt)fswProc.addTask(hillTask,taskPriority=20)fswProc.addTask(sunTask,taskPriority=20)fswProc.addTask(locTask,taskPriority=20)fswProc.addTask(ctrlTask,taskPriority=10)# 2) Gravity + SPICE + Ephemerisgrav = simIncludeGravBody.gravBodyFactory()earth = grav.createEarth(); earth.isCentralBody = Truesun = grav.createSun()spice = grav.createSpiceInterface(time="2020 JAN 01 12:00:00 (UTC)")sim.AddModelToTask("DynTask", spice, -1)grav.spiceObject.zeroBase = 'Earth'ephemObj = ephemerisConverter.EphemerisConverter(); ephemObj.ModelTag = 'Ephem'ephemObj.addSpiceInputMsg(grav.spiceObject.planetStateOutMsgs[0])  # Earthsim.AddModelToTask("DynTask", ephemObj, 190)# 3) Spacecraft + Navsc = spacecraft.Spacecraft(); sc.ModelTag = "SC"sim.AddModelToTask("DynTask", sc, 200)# Orbitmu = earth.muoe = orbitalMotion.ClassicElements()oe.a = 7028.14e3; oe.e = 0.0; oe.i = np.deg2rad(97.991)oe.Omega = np.deg2rad(272.366); oe.omega = 0.0; oe.f = 0.0rN, vN = orbitalMotion.elem2rv(mu, oe)sc.hub.r_CN_NInit = rN; sc.hub.v_CN_NInit = vNsc.hub.sigma_BNInit = [[0.1],[0.05],[-0.1]]; sc.hub.omega_BN_BInit = [[0.001],[-0.001],[0.002]]I = [900.,0.,0., 0.,800.,0., 0.,0.,600.]sc.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I)grav.addBodiesTo(sc)# NavsNav = simpleNav.SimpleNav(); sNav.ModelTag = "nav"sNav.scStateInMsg.subscribeTo(sc.scStateOutMsg)sim.AddModelToTask("DynTask", sNav, 210)# 4) Environment for sun-safe & visibilityecl = eclipse.Eclipse(); ecl.ModelTag = "eclipse"ecl.addSpacecraftToModel(sc.scStateOutMsg)ecl.addPlanetToModel(grav.spiceObject.planetStateOutMsgs[0])ecl.sunInMsg.subscribeTo(grav.spiceObject.planetStateOutMsgs[1])sim.AddModelToTask("DynTask", ecl, 180)# CSS constellation — mirror definition used in sunSafePointOnly.pycssArray = coarseSunSensor.CSSConstellation(); cssArray.ModelTag='cssConstellation'nHat_B_List = [[1.0, 0.0, 0.0],   # +X[-1.0, 0.0, 0.0],  # -X[0.0, 1.0, 0.0],   # +Y[0.0, -1.0, 0.0],  # -Y[0.0, 0.0, 1.0],   # +Z[0.0, 0.0, -1.0]   # -Z]css_list = []for i, nHat in enumerate(nHat_B_List, start=1):CSS = coarseSunSensor.CoarseSunSensor()CSS.ModelTag = f"CSS{i}"CSS.fov = 90.*mc.D2RCSS.scaleFactor = 1.0CSS.nHat_B = np.array(nHat)CSS.sunInMsg.subscribeTo(grav.spiceObject.planetStateOutMsgs[1])CSS.stateInMsg.subscribeTo(sc.scStateOutMsg)CSS.sunEclipseInMsg.subscribeTo(ecl.eclipseOutMsgs[0])css_list.append(CSS)sim.AddModelToTask("DynTask", CSS, 175)cssArray.sensorList = coarseSunSensor.CSSVector(css_list)sim.AddModelToTask("DynTask", cssArray, 170)# Ground location for locationPointingground = groundLocation.GroundLocation(); ground.ModelTag = 'GS'ground.planetRadius = orbitalMotion.REQ_EARTH*1e3ground.specifyLocation(np.deg2rad(40.0), np.deg2rad(116.0), 0.0)ground.minimumElevation = np.deg2rad(10.0); ground.maximumRange = 1e12ground.addSpacecraftToModel(sc.scStateOutMsg)ground.planetInMsg.subscribeTo(grav.spiceObject.planetStateOutMsgs[0])sim.AddModelToTask("DynTask", ground, 160)# 5) RWs + mapping + vehicle configrwFactory = simIncludeRW.rwFactory(); var = messaging.BalancedWheelsfor axis in ([1,0,0],[0,1,0],[0,0,1]):rwFactory.create('Honeywell_HR16', axis, maxMomentum=50., Omega=100., RWModel=var, useMaxTorque=True)rwEff = reactionWheelStateEffector.ReactionWheelStateEffector(); rwEff.ModelTag='RWs'rwFactory.addToSpacecraft(sc.ModelTag, rwEff, sc)sim.AddModelToTask("DynTask", rwEff, 40)rwMap = rwMotorTorque.rwMotorTorque(); rwMap.ModelTag='rwMap'rwMap.controlAxes_B = [1,0,0, 0,1,0, 0,0,1]fswRwParamMsg = rwFactory.getConfigMessage()rwMap.rwParamsInMsg.subscribeTo(fswRwParamMsg)sim.AddModelToTask("CtrlTask", rwMap, 30)vehCfg = messaging.VehicleConfigMsgPayload(); vehCfg.ISCPntB_B = IvehCfgMsg = messaging.VehicleConfigMsg().write(vehCfg)# 6) Guidance modules (three modes)# Hillhpt = hillPoint.hillPoint(); hpt.ModelTag='hillPoint'hpt.transNavInMsg.subscribeTo(sNav.transOutMsg)hpt.celBodyInMsg.subscribeTo(ephemObj.ephemOutMsgs[0])sim.AddModelToTask("HillTask", hpt, 80)hillTE = attTrackingError.attTrackingError(); hillTE.ModelTag='hillTE'hillTE.attRefInMsg.subscribeTo(hpt.attRefOutMsg)hillTE.attNavInMsg.subscribeTo(sNav.attOutMsg)sim.AddModelToTask("HillTask", hillTE, 70)# Sun safe# CSS WLScfg = messaging.CSSConfigMsgPayload()cssCfgList = []for v in nHat_B_List:unit = messaging.CSSUnitConfigMsgPayload()unit.CBias = 0.1unit.nHat_B = vcssCfgList.append(unit)cfg.cssVals = cssCfgListcfg.nCSS = len(cssCfgList)cssCfgMsg = messaging.CSSConfigMsg().write(cfg)wls = cssWlsEst.cssWlsEst(); wls.ModelTag='wls'wls.cssDataInMsg.subscribeTo(cssArray.constellationOutMsg)wls.cssConfigInMsg.subscribeTo(cssCfgMsg)sim.AddModelToTask("SunTask", wls, 60)spoint = sunSafePoint.sunSafePoint(); spoint.ModelTag='sunSafe'spoint.sHatBdyCmd = [1.0, 0.0, 0.0]  # +X boresightspoint.imuInMsg.subscribeTo(sNav.attOutMsg)spoint.sunDirectionInMsg.subscribeTo(wls.navStateOutMsg)sim.AddModelToTask("SunTask", spoint, 50)# Ground pointingloc = locationPointing.locationPointing(); loc.ModelTag='locPoint'loc.pHat_B = [0.0, 0.0, 1.0]loc.scAttInMsg.subscribeTo(sNav.attOutMsg)loc.scTransInMsg.subscribeTo(sNav.transOutMsg)loc.locationInMsg.subscribeTo(ground.currentGroundStateOutMsg)sim.AddModelToTask("LocTask", loc, 80)locTE = attTrackingError.attTrackingError(); locTE.ModelTag='locTE'locTE.attRefInMsg.subscribeTo(loc.attRefOutMsg)locTE.attNavInMsg.subscribeTo(sNav.attOutMsg)sim.AddModelToTask("LocTask", locTE, 70)# 7) AttGuid gateway (C-wrapped msg) + controller# 使用 C 消息网关以匹配 *_C_addAuthor 的参数类型guidGateway = messaging.AttGuidMsg_C().init()# 初始化一次以避免首步未写入读错_initGuid = messaging.AttGuidMsgPayload()guidGateway.write(_initGuid)messaging.AttGuidMsg_C_addAuthor(hillTE.attGuidOutMsg, guidGateway)messaging.AttGuidMsg_C_addAuthor(spoint.attGuidanceOutMsg, guidGateway)messaging.AttGuidMsg_C_addAuthor(locTE.attGuidOutMsg, guidGateway)ctrl = mrpFeedback.mrpFeedback(); ctrl.ModelTag='mrp'ctrl.guidInMsg.subscribeTo(guidGateway)ctrl.rwParamsInMsg.subscribeTo(fswRwParamMsg)ctrl.rwSpeedsInMsg.subscribeTo(rwEff.rwSpeedOutMsg)ctrl.vehConfigInMsg.subscribeTo(vehCfgMsg)ctrl.K = 1.5; ctrl.P = 8.0; ctrl.Ki = 0.0# 将控制器放到独立的 FSW 任务，保证在各制导作者之后执行sim.AddModelToTask("CtrlTask", ctrl, 65)rwMap.vehControlInMsg.subscribeTo(ctrl.cmdTorqueOutMsg)rwEff.rwMotorCmdInMsg.subscribeTo(rwMap.rwMotorTorqueOutMsg)# 9) Recorders for analysis/visualizationguidLog = guidGateway.recorder()rwSpeedLog = rwEff.rwSpeedOutMsg.recorder()sunVecLog = wls.navStateOutMsg.recorder()navAttLog = sNav.attOutMsg.recorder()navTransLog = sNav.transOutMsg.recorder()groundStateLog = ground.currentGroundStateOutMsg.recorder()ecLog = ecl.eclipseOutMsgs[0].recorder()sim.AddModelToTask("CtrlTask", guidLog)sim.AddModelToTask("DynTask", rwSpeedLog)sim.AddModelToTask("SunTask", sunVecLog)sim.AddModelToTask("DynTask", navAttLog)sim.AddModelToTask("DynTask", navTransLog)sim.AddModelToTask("DynTask", groundStateLog)sim.AddModelToTask("DynTask", ecLog)# 8) Mode schedulemodes = ["HILL", "SUN", "GROUND"]seg_ns = mc.min2nano(segment_min)total_ns = mc.hour2nano(sim_hours)# Initially only enable first modesim.enableTask("HillTask")sim.disableTask("SunTask")sim.disableTask("LocTask")t = 0k = 0while t < total_ns:mode = modes[k % 3]if mode == "HILL":sim.enableTask("HillTask"); sim.disableTask("SunTask"); sim.disableTask("LocTask")elif mode == "SUN":sim.disableTask("HillTask"); sim.enableTask("SunTask"); sim.disableTask("LocTask")else:sim.disableTask("HillTask"); sim.disableTask("SunTask"); sim.enableTask("LocTask")# first time initialize just before first Executeif t == 0:sim.InitializeSimulation()stop = min(t + seg_ns, total_ns)sim.ConfigureStopTime(stop)print(f"Executing mode {mode} until {stop*mc.NANO2MIN:.1f} min")sim.ExecuteSimulation()# Per-segment analysis and plottingif mode == "HILL":hillResult(navAttLog, navTransLog, t, stop)elif mode == "SUN":sunResult(sunVecLog, spoint, ecLog, t, stop)else:locResult(navAttLog, navTransLog, groundStateLog, loc, t, stop)t = stop; k += 1print("Finished 3-mode switching simulation")# All segments processed# Global eclipse visualization over entire simulationtry:t_ec_all = ecLog.times() * mc.NANO2MINsf_all = np.asarray(ecLog.shadowFactor).flatten()plt.figure()plt.plot(t_ec_all, sf_all, 'k-')plt.ylim([-0.05, 1.05])plt.ylabel('Shadow Factor')plt.xlabel('Time (min)')plt.title('Eclipse shadow factor (entire simulation)')plt.grid(True)plt.show()except Exception:passdef _segment_mask(t_ns, t0, t1):return (t_ns >= t0) & (t_ns <= t1)def sunResult(sunVecLog, sunPointModule, ecLog, t0, t1, threshold_deg=1.0):try:t = sunVecLog.times()v = np.asarray(sunVecLog.vehSunPntBdy)except Exception:print("[SUN] no data in this segment")return# Eclipse shadow factor in same segment; skip samples with shadowFactor <= 0try:t_ec = ecLog.times()sf = np.asarray(ecLog.shadowFactor).flatten()except Exception:t_ec = np.array([]); sf = np.array([])mask = _segment_mask(t, t0, t1)if not np.any(mask):print("[SUN] no samples in segment")returnv = v[mask]ts = t[mask]# if eclipse data available, align lengths and filter out eclipse samples (sf <= 0)if t_ec.size > 0 and sf.size == t_ec.size:m_ec = _segment_mask(t_ec, t0, t1)if np.any(m_ec):# naive alignment by lengthn = min(np.sum(mask), np.sum(m_ec))v = v[:n]ts = ts[:n]sf_seg = sf[m_ec][:n]good = sf_seg > 0.0if not np.any(good):print("[SUN] all samples in eclipse; skip segment")returnv = v[good]ts = ts[good]t_ec_seg_min = (t_ec[m_ec][:n]) * mc.NANO2MINsf_seg_plot = sf_segelse:t_ec_seg_min = np.array([]); sf_seg_plot = np.array([])else:t_ec_seg_min = np.array([]); sf_seg_plot = np.array([])ts_min = ts * mc.NANO2MIN# desired boresight from sunSafePoint configd_b = np.asarray(sunPointModule.sHatBdyCmd, dtype=float)d_b = d_b / (np.linalg.norm(d_b) + 1e-12)# sun vector v is already in body frame; angle between v and desired d_bdots = np.clip(np.sum(v * d_b[None, :], axis=1) / np.maximum(np.linalg.norm(v, axis=1), 1e-12), -1.0, 1.0)ang = np.degrees(np.arccos(dots))final = float(ang[-1])status = "PASS" if final < threshold_deg else "FAIL"print(f"[SUN] {ts_min[0]:.2f}-{ts_min[-1]:.2f} min: final align {final:.2f} deg -> {status}")fig, ax1 = plt.subplots()ax1.plot(ts_min, ang, 'r-')ax1.axhline(threshold_deg, color='k', linestyle='--', alpha=0.5)ax1.set_ylabel('Sun align (deg)')ax1.set_xlabel('Time (min)')ax1.set_title('SUN segment alignment + eclipse')ax1.grid(True)if t_ec_seg_min.size > 0:ax2 = ax1.twinx()ax2.plot(t_ec_seg_min, sf_seg_plot, 'k--', alpha=0.7)ax2.set_ylabel('Shadow Factor')ax2.set_ylim([-0.05, 1.05])plt.tight_layout()try:plt.show()except Exception:passtry:plt.show()except Exception:passdef _get_body_x_from_att(navAttLog):t = navAttLog.times()# nav att provides sigma_BN; compute body x-axis in inertial: a1_N = C_{N->B}^T e1_Btry:sigmas = np.asarray(navAttLog.sigma_BN)except Exception:return np.array([]), np.zeros((0,3))a1_list = []for s in sigmas:# convert MRP to DCM BN, then take its first row transposed (body x in N)C_BN = rbk.MRP2C(s)a1_N = C_BN.T @ np.array([1.0, 0.0, 0.0])a1_list.append(a1_N)return t, np.asarray(a1_list)def hillResult(navAttLog, navTransLog, t0, t1, threshold_deg=1.0):# Body x-axis vs nadir direction r_N/rt_att, a1_N = _get_body_x_from_att(navAttLog)try:t_trans = navTransLog.times()r_BN_N = np.asarray(navTransLog.r_BN_N)except Exception:print("[HILL] no data in this segment")returnmask = _segment_mask(t_att, t0, t1)if not np.any(mask):print("[HILL] no samples in segment")return# interpolate/align by nearest indices (assuming same dt, both in DynTask)tseg = t_att[mask]a1 = a1_N[mask]# get matching r samplesm2 = _segment_mask(t_trans, t0, t1)if not np.any(m2):print("[HILL] no position samples in segment")returnrseg = r_BN_N[m2]n = min(len(a1), len(rseg))a1 = a1[:n]; rseg = rseg[:n]; tplot = tseg[:n] * mc.NANO2MIN# nadir direction is r/|r|rhat = rseg / np.maximum(np.linalg.norm(rseg, axis=1, keepdims=True), 1e-12)dots = np.clip(np.sum(a1 * rhat, axis=1), -1.0, 1.0)ang = np.degrees(np.arccos(dots))final = float(ang[-1])status = "PASS" if final < threshold_deg else "FAIL"print(f"[HILL] {tplot[0]:.2f}-{tplot[-1]:.2f} min: final angle {final:.2f} deg -> {status}")plt.figure()plt.plot(tplot, ang, 'g-')plt.axhline(threshold_deg, color='k', linestyle='--', alpha=0.5)plt.ylabel('Angle(x_B, nadir) (deg)')plt.xlabel('Time (min)')plt.title('HILL segment: x_B vs nadir')plt.grid(True)try:plt.show()except Exception:passdef locResult(navAttLog, navTransLog, groundStateLog, locModule, t0, t1, threshold_deg=1.0):# Desired boresight is locModule.pHat_B (body axis), check alignment with line-of-sight to ground targetp_b = np.array(locModule.pHat_B, dtype=float)# body axis in inertial: a_N = C_BN^T p_Bt_att, aN = _get_body_axis_from_att(navAttLog, p_b)try:t_trans = navTransLog.times()r_BN_N = np.asarray(navTransLog.r_BN_N)t_g = groundStateLog.times()r_LN_N = np.asarray(groundStateLog.r_LN_N)except Exception:print("[GROUND] no data in this segment")return# segment masksm_att = _segment_mask(t_att, t0, t1)m_tr = _segment_mask(t_trans, t0, t1)m_g = _segment_mask(t_g, t0, t1)if not (np.any(m_att) and np.any(m_tr) and np.any(m_g)):print("[GROUND] insufficient samples in segment")returnaN = aN[m_att]; tseg = t_att[m_att]rN = r_BN_N[m_tr]rL = r_LN_N[m_g]# align by length (assumes equal dt); otherwise could resample by timen = min(len(aN), len(rN), len(rL))aN = aN[:n]; rN = rN[:n]; rL = rL[:n]; tplot = tseg[:n] * mc.NANO2MIN# line of sight from sc to ground: r_LN_N - r_BN_Nlos = rL - rNlos_hat = los / np.maximum(np.linalg.norm(los, axis=1, keepdims=True), 1e-12)dots = np.clip(np.sum(aN * los_hat, axis=1), -1.0, 1.0)ang = np.degrees(np.arccos(dots))final = float(ang[-1])status = "PASS" if final < threshold_deg else "FAIL"print(f"[GROUND] {tplot[0]:.2f}-{tplot[-1]:.2f} min: final angle {final:.2f} deg -> {status}")plt.figure()plt.plot(tplot, ang, 'b-')plt.axhline(threshold_deg, color='k', linestyle='--', alpha=0.5)plt.ylabel('Angle(boresight, LOS) (deg)')plt.xlabel('Time (min)')plt.title('GROUND segment: boresight vs LOS')plt.grid(True)try:plt.show()except Exception:passdef _get_body_axis_from_att(navAttLog, axis_b):t = navAttLog.times()try:sigmas = np.asarray(navAttLog.sigma_BN)except Exception:return np.array([]), np.zeros((0,3))a_list = []e_b = np.asarray(axis_b, dtype=float)for s in sigmas:C_BN = rbk.MRP2C(s)a_N = C_BN.T @ e_ba_list.append(a_N)return t, np.asarray(a_list)if __name__ == "__main__":run_mode_switch(sim_hours=6.0, segment_min=30.0)

查看全文

http://www.dtcms.com/a/360624.html