网页制作公司网站国内搜索引擎

网页制作公司网站,国内搜索引擎,昆明网站排名优化报价,网站推广优化流程下面分三大部分说明#xff1a; 数学推导与公式#xff08;欧拉角 ↔ 四元数#xff09;C 实现代码Python 实现代码 为便于说明#xff0c;统一采用常用的航空/机器人学约定#xff1a; 欧拉角顺序#xff1a;Z-Y-X#xff08;yaw-pitch-roll#xff09;旋转顺序#…下面分三大部分说明数学推导与公式欧拉角 ↔ 四元数C 实现代码Python 实现代码为便于说明统一采用常用的航空/机器人学约定欧拉角顺序Z-Y-Xyaw-pitch-roll旋转顺序先绕X轴转 roll再绕Y轴转 pitch最后绕Z轴转 yawRRz(yaw) Ry(pitch) Rx(roll) R R_z(\text{yaw}) \, R_y(\text{pitch}) \, R_x(\text{roll})RRz​(yaw)Ry​(pitch)Rx​(roll)欧拉角记作roll绕x轴旋转记为 ( \phi ) 或 rollpitch绕y轴旋转记为 ( \theta ) 或 pitchyaw绕z轴旋转记为 ( \psi ) 或 yaw四元数记作qwx iy jz k q w x\,\mathbf{i} y\,\mathbf{j} z\,\mathbf{k}qwxiyjzk通常写为(w,x,y,z)(w, x, y, z)(w,x,y,z)为单位四元数。注意不同库ROS、Unity、DirectX、GLM、Eigen 等可能有不同的轴定义、欧拉角顺序和左/右手坐标系要确保公式与使用的库约定一致。下面是右手坐标系主动旋转rotate vector语境。一、欧拉角 ↔ 四元数数学推导与公式1.1 基本概念回顾欧拉角Euler Angles使用三个有序旋转绕坐标轴描述三维空间中的任意姿态roll绕 xpitch绕 yyaw绕 z优点直观类似飞机的滚转、俯仰、偏航缺点存在万向节锁gimbal lock、插值不平滑、组合复杂。四元数Quaternion单位四元数q(w,x,y,z)q (w, x, y, z)q(w,x,y,z)描述旋转也可用轴角形式表达q(cos⁡θ2, sin⁡θ2 u^x, sin⁡θ2 u^y, sin⁡θ2 u^z) q \left( \cos\frac{\theta}{2}, \; \sin\frac{\theta}{2}\,\hat{u}_x , \; \sin\frac{\theta}{2}\,\hat{u}_y , \; \sin\frac{\theta}{2}\,\hat{u}_z \right)q(cos2θ​,sin2θ​u^x​,sin2θ​u^y​,sin2θ​u^z​)其中u^\hat{u}u^为单位旋转轴θ\thetaθ为旋转角度优点无万向节锁、易于插值slerp、组合为四元数乘法。1.2 单轴旋转的四元数绕坐标轴的旋转右手定则绕 x 轴转角α\alphaαqx(α)(cos⁡α2, sin⁡α2, 0, 0) q_x(\alpha) \left( \cos\frac{\alpha}{2},\ \sin\frac{\alpha}{2},\ 0,\ 0 \right)qx​(α)(cos2α​,sin2α​,0,0)绕 y 轴转角β\betaβqy(β)(cos⁡β2, 0, sin⁡β2, 0) q_y(\beta) \left( \cos\frac{\beta}{2},\ 0,\ \sin\frac{\beta}{2},\ 0 \right)qy​(β)(cos2β​,0,sin2β​,0)绕 z 轴转角γ\gammaγqz(γ)(cos⁡γ2, 0, 0, sin⁡γ2) q_z(\gamma) \left( \cos\frac{\gamma}{2},\ 0,\ 0,\ \sin\frac{\gamma}{2} \right)qz​(γ)(cos2γ​,0,0,sin2γ​)旋转组合对应四元数乘法右乘约定为“后旋转写在左边”或“先执行的在右边”要统一对于RRz(ψ) Ry(θ) Rx(ϕ) R R_z(\psi)\,R_y(\theta)\,R_x(\phi)RRz​(ψ)Ry​(θ)Rx​(ϕ)对应的四元数是qqz(ψ) qy(θ) qx(ϕ) q q_z(\psi)\, q_y(\theta)\, q_x(\phi)qqz​(ψ)qy​(θ)qx​(ϕ)1.3 欧拉角 (roll, pitch, yaw) → 四元数 (w, x, y, z)令单位弧度roll ϕ\phiϕpitch θ\thetaθyaw ψ\psiψ先计算半角的三角函数cϕcos⁡ϕ2,sϕsin⁡ϕ2cθcos⁡θ2,sθsin⁡θ2cψcos⁡ψ2,sψsin⁡ψ2 \begin{aligned} c_\phi \cos\frac{\phi}{2}, s_\phi \sin\frac{\phi}{2} \\ c_\theta \cos\frac{\theta}{2}, s_\theta \sin\frac{\theta}{2} \\ c_\psi \cos\frac{\psi}{2}, s_\psi \sin\frac{\psi}{2} \end{aligned}cϕ​cθ​cψ​​cos2ϕ​,cos2θ​,cos2ψ​,​sϕ​sθ​sψ​​sin2ϕ​sin2θ​sin2ψ​​按照qqz(ψ) qy(θ) qx(ϕ) q q_z(\psi)\, q_y(\theta)\, q_x(\phi)qqz​(ψ)qy​(θ)qx​(ϕ)推导结果为Z-Y-Xyaw-pitch-rollwcϕcθcψsϕsθsψxsϕcθcψ−cϕsθsψycϕsθcψsϕcθsψzcϕcθsψ−sϕsθcψ \begin{aligned} w c_\phi c_\theta c_\psi s_\phi s_\theta s_\psi \\ x s_\phi c_\theta c_\psi - c_\phi s_\theta s_\psi \\ y c_\phi s_\theta c_\psi s_\phi c_\theta s_\psi \\ z c_\phi c_\theta s_\psi - s_\phi s_\theta c_\psi \end{aligned}wxyz​cϕ​cθ​cψ​sϕ​sθ​sψ​sϕ​cθ​cψ​−cϕ​sθ​sψ​cϕ​sθ​cψ​sϕ​cθ​sψ​cϕ​cθ​sψ​−sϕ​sθ​cψ​​习惯上返回顺序为 (w, x, y, z)。这是极其常用的一组公式例如 ROS、Eigen、许多游戏/机器人代码都采用此约定的某种变体。如果你的应用使用了不同顺序XYZ、ZYX 等公式会相应改变不要混用。1.4 四元数 (w, x, y, z) → 欧拉角 (roll, pitch, yaw)现在给定单位四元数q(w,x,y,z) q (w, x, y, z)q(w,x,y,z)目标是恢复 Z-Y-X 顺序的欧拉角yaw-pitch-roll。先给出最终常用公式再解释细节。常用公式Z-Y-Xrollϕatan2⁡(2(wxyz), 1−2(x2y2))pitchθarcsin⁡(2(wy−zx))yawψatan2⁡(2(wzxy), 1−2(y2z2)) \begin{aligned} \text{roll} \phi \operatorname{atan2}\big( 2(w x y z),\ 1 - 2(x^2 y^2) \big) \\ \text{pitch} \theta \arcsin\big( 2(w y - z x) \big) \\ \text{yaw} \psi \operatorname{atan2}\big( 2(w z x y),\ 1 - 2(y^2 z^2) \big) \end{aligned}rollpitchyaw​ϕatan2(2(wxyz),1−2(x2y2))θarcsin(2(wy−zx))ψatan2(2(wzxy),1−2(y2z2))​但是要注意数值误差导致的越界问题中间项s2(wy−zx) s 2(w y - z x)s2(wy−zx)理论上s∈[−1,1]s \in [-1, 1]s∈[−1,1]实际浮点运算可能略超出所以通常需要s 2 * (w*y - z*x) if s 1: pitch pi/2 elif s -1: pitch -pi/2 else: pitch asin(s)这是为了处理接近万向节锁时的稳健性。简要推导思路可略读从四元数得到旋转矩阵 (R)再从矩阵提取欧拉角。单位四元数对应的旋转矩阵为R[1−2(y2z2)2(xy−zw)2(xzyw)2(xyzw)1−2(x2z2)2(yz−xw)2(xz−yw)2(yzxw)1−2(x2y2)] R \begin{bmatrix} 1 - 2(y^2 z^2) 2(x y - z w) 2(x z y w) \\ 2(x y z w) 1 - 2(x^2 z^2) 2(y z - x w) \\ 2(x z - y w) 2(y z x w) 1 - 2(x^2 y^2) \end{bmatrix}R​1−2(y2z2)2(xyzw)2(xz−yw)​2(xy−zw)1−2(x2z2)2(yzxw)​2(xzyw)2(yz−xw)1−2(x2y2)​​在 Z-Y-X 约定下RRz(ψ)Ry(θ)Rx(ϕ) R R_z(\psi) R_y(\theta) R_x(\phi)RRz​(ψ)Ry​(θ)Rx​(ϕ)对该矩阵逐项比较就可以推导出上述 atan2 / asin 公式。由于推导较繁琐在工程代码中通常直接使用已知的标准形式。二、C 实现示例下面给出不依赖第三方库的简单 C 实现仅使用cmath约定输入输出的欧拉角单位为弧度欧拉角顺序输入/输出按(roll, pitch, yaw)四元数顺序(w, x, y, z)如需使用度数可额外写转换函数deg2rad/rad2deg。2.1 C欧拉角 → 四元数#includecmath#includetuplestructQuaternion{doublew,x,y,z;};// 输入roll, pitch, yaw (弧度)// 输出单位四元数 (w, x, y, z)// 约定Z-Y-X (yaw-pitch-roll) 旋转顺序Quaternioneuler_to_quaternion(doubleroll,doublepitch,doubleyaw){doublehalf_rollroll*0.5;doublehalf_pitchpitch*0.5;doublehalf_yawyaw*0.5;doublec1std::cos(half_roll);doubles1std::sin(half_roll);doublec2std::cos(half_pitch);doubles2std::sin(half_pitch);doublec3std::cos(half_yaw);doubles3std::sin(half_yaw);Quaternion q;q.wc1*c2*c3s1*s2*s3;q.xs1*c2*c3-c1*s2*s3;q.yc1*s2*c3s1*c2*s3;q.zc1*c2*s3-s1*s2*c3;returnq;}2.2 C四元数 → 欧拉角#includecmath#includetuplestructQuaternion{doublew,x,y,z;};// 输出roll, pitch, yaw (弧度)// 约定与 euler_to_quaternion 保持一致Z-Y-X (yaw-pitch-roll)std::tupledouble,double,doublequaternion_to_euler(constQuaternionq){doublewq.w;doublexq.x;doubleyq.y;doublezq.z;// roll (x-axis rotation)doublesinr_cosp2.0*(w*xy*z);doublecosr_cosp1.0-2.0*(x*xy*y);doublerollstd::atan2(sinr_cosp,cosr_cosp);// pitch (y-axis rotation)doublesinp2.0*(w*y-z*x);doublepitch;if(std::fabs(sinp)1.0){// use 90 degrees if out of rangepitchstd::copysign(M_PI/2.0,sinp);}else{pitchstd::asin(sinp);}// yaw (z-axis rotation)doublesiny_cosp2.0*(w*zx*y);doublecosy_cosp1.0-2.0*(y*yz*z);doubleyawstd::atan2(siny_cosp,cosy_cosp);returnstd::make_tuple(roll,pitch,yaw);}2.3 可选角度与弧度转换C如果你更习惯用角度制constexprdoublePI3.14159265358979323846;inlinedoubledeg2rad(doubledeg){returndeg*PI/180.0;}inlinedoublerad2deg(doublerad){returnrad*180.0/PI;}使用时注意统一单位例如doubleroll_deg30.0;doublepitch_deg10.0;doubleyaw_deg45.0;Quaternion qeuler_to_quaternion(deg2rad(roll_deg),deg2rad(pitch_deg),deg2rad(yaw_deg));三、Python 实现示例Python 实现会更简洁下面给出纯 numpy 实现不依赖外部库。如果你使用scipy或transformations.py可以直接调用现成函数不过这里展示“从零实现”的版本以说明公式。3.1 Python欧拉角 → 四元数importnumpyasnpdefeuler_to_quaternion(roll,pitch,yaw): 欧拉角 (roll, pitch, yaw) - 四元数 (w, x, y, z) 约定Z-Y-X (yaw-pitch-roll)弧度制 half_rollroll*0.5half_pitchpitch*0.5half_yawyaw*0.5c1np.cos(half_roll)s1np.sin(half_roll)c2np.cos(half_pitch)s2np.sin(half_pitch)c3np.cos(half_yaw)s3np.sin(half_yaw)wc1*c2*c3s1*s2*s3 xs1*c2*c3-c1*s2*s3 yc1*s2*c3s1*c2*s3 zc1*c2*s3-s1*s2*c3returnnp.array([w,x,y,z],dtypefloat)3.2 Python四元数 → 欧拉角importnumpyasnpdefquaternion_to_euler(q): 四元数 (w, x, y, z) - 欧拉角 (roll, pitch, yaw) 约定与 euler_to_quaternion 一致Z-Y-X (yaw-pitch-roll)弧度制 qnp.asarray(q,dtypefloat)w,x,y,zq# roll (x-axis)sinr_cosp2.0*(w*xy*z)cosr_cosp1.0-2.0*(x*xy*y)rollnp.arctan2(sinr_cosp,cosr_cosp)# pitch (y-axis)sinp2.0*(w*y-z*x)ifnp.abs(sinp)1.0:pitchnp.sign(sinp)*np.pi/2.0else:pitchnp.arcsin(sinp)# yaw (z-axis)siny_cosp2.0*(w*zx*y)cosy_cosp1.0-2.0*(y*yz*z)yawnp.arctan2(siny_cosp,cosy_cosp)returnroll,pitch,yaw3.3 简单测试Python你可以做一个往返测试验证实现是否一致if__name____main__:# 测试用的欧拉角弧度rollnp.deg2rad(30.0)pitchnp.deg2rad(20.0)yawnp.deg2rad(60.0)qeuler_to_quaternion(roll,pitch,yaw)print(Quaternion:,q)r2,p2,y2quaternion_to_euler(q)print(Recovered (deg):,np.rad2deg([r2,p2,y2]))输出中恢复的角度应该与原始 (30, 20, 60) 非常接近浮点误差级别。四、工程实践中的注意事项简要保持约定统一坐标系右手还是左手欧拉角顺序XYZ、ZYX、ZXY 等旋转是主动还是被动坐标系旋转使用不同库时一定确认文档不然很容易出现角度“看起来不对”的问题。单位统一度 / 弧度C/C 中的三角函数使用弧度Python 的numpy也是弧度如果你的业务逻辑习惯用度数记得在接口处集中转换。归一化四元数由于数值误差反复运算后的四元数可能不再是单位长度需要定期做一次归一化doublenormstd::sqrt(w*wx*xy*yz*z);w/norm;x/norm;y/norm;z/norm;避免频繁使用欧拉角插值在动画、控制、轨迹规划中如果要在姿态之间插值尽量使用四元数插值slerp而不是直接线性插值欧拉角。