From 7c2609afd4a9f7f142ad25a5ce250e245453a725 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=91=B5=E5=91=B5=E5=93=92346?= Date: Wed, 5 Mar 2025 08:22:18 +0000 Subject: [PATCH] =?UTF-8?q?update=20AstroLib/Src/AsAttitudeParam=5F2023.cp?= =?UTF-8?q?p.=20=E5=A2=9E=E5=8A=A0=E5=9B=9B=E5=85=83=E6=95=B0=E8=BD=AC321?= =?UTF-8?q?=E8=BD=AC=E5=BA=8F=E6=AC=A7=E6=8B=89=E8=A7=92=E7=A8=8B=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: 呵呵哒346 --- AstroLib/Src/AsAttitudeParam_2023.cpp | 112 +++++++++++++++++++++++++- 1 file changed, 111 insertions(+), 1 deletion(-) diff --git a/AstroLib/Src/AsAttitudeParam_2023.cpp b/AstroLib/Src/AsAttitudeParam_2023.cpp index 5ed0c02..1db41de 100644 --- a/AstroLib/Src/AsAttitudeParam_2023.cpp +++ b/AstroLib/Src/AsAttitudeParam_2023.cpp @@ -1,4 +1,4 @@ - + #include "AsAttitudeParam_2023.h" #include "AsCoordinate.h" #include "AsMath.h" @@ -367,3 +367,113 @@ void AsMtxToEuler123(const CMatrix& mtx, euler.m_Angle3 = psi; } + + +///*********************************************************************** +/// 将四元数转换为321转序的欧拉角(Z-Y-X) +/// 遵循航天器标准定义：偏航角->俯仰角->滚转角 +/// 对应绕惯性系Z轴->新Y轴->新X轴的连续旋转 +/// @Author 王薪程（你们可爱的学长哦~） +/// @Date 2025-03-03 +/// @Version 3.2.1 +/// +/// @Input +/// @Param quat 输入四元数(需标准化，不标准化也无所谓啦），顺序:标量Qs,矢量QxQyQz) +/// +/// @Output +/// @Param euler 输出欧拉角结构体(单位：弧度) +/// m_Angle3: 绕X轴滚转角 φ ∈ [-π,π] +/// m_Angle2: 绕Y轴俯仰角 θ ∈ [-π/2,π/2] +/// m_Angle1: 绕Z轴偏航角 ψ ∈ [-π,π] +/// +/// @Return true: 转换成功 false: 四元数未标准化 +/// +/// @算法说明： +/// 1. 通过四元数导出旋转矩阵关键元素，避免完整矩阵计算 +/// 2. 采用显式代数展开提升计算效率 +/// 3. 奇异位置处理逻辑与AsMtxToEuler321()保持兼容 +/// 4. 数值阈值统一采用工程标准1e-7 +///*********************************************************************** +void AsQuatToEuler321(CQuaternion& quat, CEuler& euler) +{ + const double eps = 1e-7; // 工程计算浮点误差阈值 + euler = { 0.0, 0.0, 0.0 }; // 显式初始化输出参数 + + //============ 四元数标准化验证 ============ + const double norm_sq = quat.m_Qs * quat.m_Qs + + quat.m_Qx * quat.m_Qx + + quat.m_Qy * quat.m_Qy + + quat.m_Qz * quat.m_Qz; + if (fabs(norm_sq - 1.0) > eps) + { + printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq); + quat.Norm(); // 强制标准化四元数 + } + + //======= 计算旋转矩阵关键元素(代数展开式) ======= + const double q0 = quat.m_Qs; // 标量部分 + const double q1 = quat.m_Qx; + const double q2 = quat.m_Qy; + const double q3 = quat.m_Qz; + + /* 旋转矩阵R(321转序)关键元素 + [ cy*cz cy*sz -sy ] + [ sy*sx*cz-sz*cx sy*sx*sz+cz*cx cy*sx ] + [ sy*cx*cz+sz*sx sy*cx*sz-cz*sx cy*cx ] + */ + // 仅计算必要元素以提升性能 + double R02 = 2 * (q1 * q3 - q0 * q2); // -sy*cy ≈ -sin(theta) + double R00 = 1 - 2 * (q2 * q2 + q3 * q3); // cy*cz + double R01 = 2 * (q1 * q2 + q0 * q3); // cy*sz + double R12 = 2 * (q2 * q3 + q0 * q1); // cy*sx + double R22 = 1 - 2 * (q1 * q1 + q2 * q2); // cy*cx + + //========= 俯仰角计算(保证θ∈[-π/2,π/2]) ========= + if (R02 > 1) R02 = 1; // 防止数值误差导致acos溢出 + if (R02 < -1) R02 = -1; // 防止数值误差导致acos溢出 + euler.m_Angle2 = -asin(R02); // 对应321转序第二个旋转 + + //======= 万向节锁处理(θ≈±π/2时) ======= + if (fabs(R02) > 1.0 - eps) + { + /* 奇异情况处理逻辑 + 当θ=±π/2时，滚转角φ与偏航角ψ不可唯一确定 + 约定固定ψ=0，通过矩阵元素计算φ + 与方向余弦阵转换函数AsMtxToEuler321()保持兼容 + */ + euler.m_Angle1 = 0; // 固定偏航角 + + // 根据R22符号确定θ的正负 + const double sign_theta = (R02 > 0) ? -1.0 : 1.0; + + /* 计算滚转角φ + 使用R12和R22元素： + sy*cx = R12/(2*cy) + cy*cx = R22 + 当θ→±π/2时，cy→0，改用q0和q1计算 + */ + euler.m_Angle3 = 2 * atan2(q1, q0); + } + else + { + //======= 常规情况计算 ======== + // 偏航角ψ(z轴): atan2(cy*sz, cy*cz) + euler.m_Angle1 = atan2(R01, R00); + + // 滚转角φ(x轴): atan2(cy*sx, cy*cx) + euler.m_Angle3 = atan2(R12, R22); + } + + //===== 角度范围修正 ===== + // 保证ψ∈[-π,π]，φ∈[-π,π] + if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI; + if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI; + if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI; + if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI; + + //// 测试用，转为角度制 + //euler.m_Angle3 = euler.m_Angle3 * 180 / AsCPI; + //euler.m_Angle2 = euler.m_Angle2 * 180 / AsCPI; + //euler.m_Angle1 = euler.m_Angle1 * 180 / AsCPI; +} + -- Gitee