대한민국 425 사업 초소형 SAR 군집위성 고기동 CMG 3축 자세 지향 및 특이점 회피 1,000Hz 0-Alloc C# 위성자세제어 엔진

제1장: 4기 피라미드 SGCMG 기하학 및 자이로 각운동량 수렴

경사각 $\beta = 54.74^\circ$로 배치된 4개의 SGCMG가 고속 휠 각운동량 $h_0$를 유지한 채 짐벌각 $\boldsymbol{\delta} = [\delta_1, \delta_2, \delta_3, \delta_4]^T$로 회전할 때, 위성 본체에 작용하는 자이로 토크 $\boldsymbol{\tau}_{\text{cmg}}$와 야코비안 행렬 $\mathbf{A}(\boldsymbol{\delta})$는 다음과 같습니다:

제2장: 특이점(Singularity) 판별 및 SR-Inverse 강건 조향 법칙

행렬식 $m = \det(\mathbf{A} \mathbf{A}^T) = 0$이 되는 특이 영역에서는 일반 무어-펜로즈 역행렬이 발산하므로, 감쇠 최소자승법 기반의 **SR-Inverse(Singularity Robust Inverse)**를 적용합니다:

제3장: 1,000Hz 0-Alloc C# 425 위성 CMG 자세제어 엔진 아키텍처

우주 저궤도 환경에서 1ms 주기로 쿼터니언 오차와 4축 짐벌 지령을 무할당 연산하는 순수 C# 구조체입니다:

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Satellite425
{
    /// <summary>
    /// 위성 3축 자세 쿼터니언 및 각속도 상태 (0-Alloc)
    /// </summary>
    public readonly struct SatelliteAttitudeState
    {
        public readonly Quaternion CurrentQuat; // 현재 위성 자세 쿼터니언 q (J2000 좌표계)
        public readonly Quaternion TargetQuat;  // 목표 관측 자세 쿼터니언 q_tgt
        public readonly Vector3 BodyAngularVel; // 동체 각속도 w_b (rad/s)
        public readonly Vector4 GimbalAngles;   // 4개 CMG 현재 짐벌각 delta_1~4 (rad)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SatelliteAttitudeState(Quaternion q, Quaternion qTgt, Vector3 w, Vector4 gimbals)
        {
            CurrentQuat = q;
            TargetQuat = qTgt;
            BodyAngularVel = w;
            GimbalAngles = gimbals;
        }
    }

    /// <summary>
    /// CMG 짐벌 각속도 구동 지령 구조체
    /// </summary>
    public readonly struct CmgGimbalCommand
    {
        public readonly Vector4 GimbalRatesRps; // 4개 짐벌 서보 각속도 지령 (rad/s)
        public readonly Vector3 CommandedTorque;// 요구 자세 제어 토크 (N*m)
        public readonly float SingularityMetric; // 특이점 지표 det(A*A^T)
        public readonly bool IsInSingularityZone;// 특이점 근접 회피 활성화 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CmgGimbalCommand(Vector4 rates, Vector3 torque, float metric, bool inSing)
        {
            GimbalRatesRps = rates;
            CommandedTorque = torque;
            SingularityMetric = metric;
            IsInSingularityZone = inSing;
        }
    }

    /// <summary>
    /// 1,000Hz 결정론적 425 초소형 SAR 위성 CMG 제어 엔진
    /// </summary>
    public ref struct SarSatelliteCmgAttitudeEngine
    {
        private const float H0 = 1.25f;                  // 단일 CMG 모멘텀 (1.25 N*m*s)
        private const float Beta = 54.7356f * (MathF.PI / 180.0f); // 피라미드 경사각
        private const float MaxGimbalRate = 2.5f;        // 최대 짐벌 속도 한계 (rad/s)
        private const float SingularityThreshold = 0.05f;// 특이점 경계 임계값 m0
        private const float Alpha0 = 0.025f;             // SR-Inverse 감쇠 파라미터

        private const float KpAttitude = 8.5f;           // 쿼터니언 비례 제어 이득
        private const float KdAttitude = 3.2f;           // 각속도 댐핑 제어 이득

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CmgGimbalCommand UpdateAttitudeControl(ref SatelliteAttitudeState state, float dt)
        {
            // 1. 오차 쿼터니언 계산: q_err = q_tgt * q_current^-1
            Quaternion qInv = Quaternion.Inverse(state.CurrentQuat);
            Quaternion qErr = Quaternion.Multiply(state.TargetQuat, qInv);

            // 단위원 쿼터니언 벡터부 추출
            Vector3 qVecErr = new Vector3(qErr.X, qErr.Y, qErr.Z);
            if (qErr.W < 0.0f) qVecErr = -qVecErr; // 최단 경로 회전 선택

            // 2. 요구 복원 제어 토크 산출: tau_cmd = -Kp * qVecErr - Kd * w
            Vector3 tauCmd = -(KpAttitude * qVecErr) - (KdAttitude * state.BodyAngularVel);

            // 3. CMG 야코비안 행렬 A(3x4) 성분 산출
            float cb = MathF.Cos(Beta);
            float sb = MathF.Sin(Beta);

            float s1 = MathF.Sin(state.GimbalAngles.X);
            float c1 = MathF.Cos(state.GimbalAngles.X);
            float s2 = MathF.Sin(state.GimbalAngles.Y);
            float c2 = MathF.Cos(state.GimbalAngles.Y);
            float s3 = MathF.Sin(state.GimbalAngles.Z);
            float c3 = MathF.Cos(state.GimbalAngles.Z);
            float s4 = MathF.Sin(state.GimbalAngles.W);
            float c4 = MathF.Cos(state.GimbalAngles.W);

            // A의 4개 열벡터 (a1, a2, a3, a4)
            Vector3 a1 = new Vector3(-cb * c1, -s1, sb * c1) * H0;
            Vector3 a2 = new Vector3(s2, -cb * c2, sb * c2) * H0;
            Vector3 a3 = new Vector3(cb * c3, s3, sb * c3) * H0;
            Vector3 a4 = new Vector3(-s4, cb * c4, sb * c4) * H0;

            // 4. A * A^T (3x3) 대칭 행렬 산출
            float m11 = Vector3.Dot(new Vector3(a1.X, a2.X, a3.X), new Vector3(a1.X, a2.X, a3.X)) + a4.X * a4.X;
            float m12 = a1.X * a1.Y + a2.X * a2.Y + a3.X * a3.Y + a4.X * a4.Y;
            float m13 = a1.X * a1.Z + a2.X * a2.Z + a3.X * a3.Z + a4.X * a4.Z;
            float m22 = a1.Y * a1.Y + a2.Y * a2.Y + a3.Y * a3.Y + a4.Y * a4.Y;
            float m23 = a1.Y * a1.Z + a2.Y * a2.Z + a3.Y * a3.Z + a4.Y * a4.Z;
            float m33 = a1.Z * a1.Z + a2.Z * a2.Z + a3.Z * a3.Z + a4.Z * a4.Z;

            // 행렬식 det(A*A^T)
            float det = m11 * (m22 * m33 - m23 * m23) 
                      - m12 * (m12 * m33 - m23 * m13) 
                      + m13 * (m12 * m23 - m22 * m13);

            float m = MathF.Max(det, 0.0f);
            bool inSingularity = m < SingularityThreshold;

            // SR-Inverse 감쇠 계수 alpha
            float alpha = inSingularity ? Alpha0 * MathF.Pow(1.0f - m / SingularityThreshold, 2.0f) : 0.0f;

            // 감쇠된 대칭 행렬 M_damped = M + alpha * I
            m11 += alpha;
            m22 += alpha;
            m33 += alpha;

            // 3x3 역행렬 수치 계산
            float invDet = 1.0f / MathF.Max(m11 * (m22 * m33 - m23 * m23) - m12 * (m12 * m33 - m23 * m13) + m13 * (m12 * m23 - m22 * m13), 1e-6f);
            Vector3 col1 = new Vector3(m22 * m33 - m23 * m23, -(m12 * m33 - m23 * m13), m12 * m23 - m22 * m13) * invDet;
            Vector3 col2 = new Vector3(-(m12 * m33 - m13 * m23), m11 * m33 - m13 * m13, -(m11 * m23 - m12 * m13)) * invDet;
            Vector3 col3 = new Vector3(m12 * m23 - m13 * m22, -(m11 * m23 - m13 * m12), m11 * m22 - m12 * m12) * invDet;

            // pVec = (M_damped)^-1 * (-tau_cmd)
            Vector3 negTau = -tauCmd;
            Vector3 pVec = new Vector3(
                Vector3.Dot(new Vector3(col1.X, col2.X, col3.X), negTau),
                Vector3.Dot(new Vector3(col1.Y, col2.Y, col3.Y), negTau),
                Vector3.Dot(new Vector3(col1.Z, col2.Z, col3.Z), negTau)
            );

            // delta_dot = A^T * pVec
            float r1 = Math.Clamp(Vector3.Dot(a1, pVec), -MaxGimbalRate, MaxGimbalRate);
            float r2 = Math.Clamp(Vector3.Dot(a2, pVec), -MaxGimbalRate, MaxGimbalRate);
            float r3 = Math.Clamp(Vector3.Dot(a3, pVec), -MaxGimbalRate, MaxGimbalRate);
            float r4 = Math.Clamp(Vector3.Dot(a4, pVec), -MaxGimbalRate, MaxGimbalRate);

            return new CmgGimbalCommand(new Vector4(r1, r2, r3, r4), tauCmd, m, inSingularity);
        }
    }
}

제4장: 45° 고속 선회 기동 및 특이점 회피 시계열 성능 지표

지상 표적을 긴급 지향하기 위해 위성을 45° 고속 Slew 회전시킬 때 1,000Hz CMG 제어 시계열 데이터입니다: