devbox
不正经
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

欧拉角与四元数互转，及四元数slerp球面线性插值算法

作者：不正经 | 2024-04-14 15:20:33

赞

踩

欧拉角与四元数互转，及四元数slerp球面线性插值算法

1. 欧拉角与四元数是什么？

roll：翻滚角，pitch：俯仰角，heading：航向角

roll、pitch、heading，这3个角又称为欧拉角，欧拉角是弧度。弧度与度°可以通过公式转换；

四元数：w，x，y，z，有 xx+yy+zz+ww = 1

欧拉角与四元数可以互转，四元数插值完在转回欧拉角，对于航向角突变的情况会更准确；

  • Math.toDegrees(eulerAngles.roll); // 弧度转角度
  • Math.toRadians(roll); // 角度转弧度
  • roll 范围 [-180°~180°]
  • pitch 范围 [-180°~180°]
  • heading 范围 [0°~360°]

2. 源码

2.1 欧拉角类

  1. package test;
  2.  
  3. /****************************
  4.  * Class Name： EulerAngles
  5.  * Description： <欧拉角类>
  6.  * @Author: seminar
  7.  * @create: 2021/05/21
  8.  * @since: 1.0.0
  9.  ***************************/
  10. public class EulerAngles {
  11.      
  12.     /**
  13.      * Math.toRadians(roll) 角度转弧度
  14.      * Math.toDegrees(roll) 弧度转角度
  15.      * 
  17.      * 翻滚角（roll） 弧度
  18.      */
  19.     public double roll;
  20.  
  21.     /**
  22.      * 俯仰角(pitch) 弧度
  23.      */
  24.     public double pitch;
  25.  
  26.     /**
  27.      * yaw 即heading(航向角) 弧度
  28.      */
  29.     public double yaw;
  30.  
  31.     public EulerAngles(float pitch, float yaw, float roll) {
  32.      
  33.         this.pitch = pitch;
  34.         this.yaw = yaw;
  35.         this.roll = roll;
  36.     }
  37.  
  38.     public EulerAngles(float w, float x, float y, float z) {
  39.      
  40.         // roll (x-axis rotation)
  41.         float sinr_cosp = 2 * (w * x + y * z);
  42.         float cosr_cosp = 1 - 2 * (x * x + y * y);
  43.         this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
  44.  
  45.         // pitch (y-axis rotation)
  46.         float sinp = 2 * (w * y - z * x);
  47.         if (Math.abs(sinp) >= 1) {
  48.      
  49.             this.pitch = Math.copySign(1.57075f, sinp); // use 90 degrees if out of range
  50.         } else {
  51.      
  52.             this.pitch = (float) Math.asin(sinp);
  53.         }
  54.  
  55.         // yaw (z-axis rotation)
  56.         float siny_cosp = 2 * (w * z + x * y);
  57.         float cosy_cosp = 1 - 2 * (y * y + z * z);
  58.         this.yaw = (float) Math.atan2(siny_cosp, cosy_cosp);
  59.     }
  60.  
  61.     public Quaternion toQuaternion() {
  62.      
  63.         //欧拉角转四元数，角度减半是因为四元数旋转计算时需要旋转两次，具体原理请查看四元数原理
  64.         float cy = (float) Math.cos(yaw * 0.5f);
  65.         float sy = (float) Math.sin(yaw * 0.5f);
  66.         float cp = (float) Math.cos(pitch * 0.5f);
  67.         float sp = (float) Math.sin(pitch * 0.5f);
  68.         float cr = (float) Math.cos(roll * 0.5f);
  69.         float sr = (float) Math.sin(roll * 0.5f);
  70.         Quaternion q = new Quaternion();
  71.         q.w = cy * cp * cr + sy * sp * sr;
  72.         q.x = cy * cp * sr - sy * sp * cr;
  73.         q.y = sy * cp * sr + cy * sp * cr;
  74.         q.z = sy * cp * cr - cy * sp * sr;
  75.         return q;
  76.     }
  77. }

2.2 四元数类

  1. package test;
  2.  
  3. import lombok.extern.slf4j.Slf4j;
  4.  
  5. /****************************
  6.  * Class Name： Quaternion
  7.  * Description： <四元数类>
  8.  * @Author: seminar
  9.  * @create: 2021/05/21
  10.  * @since: 1.0.0
  11.  ***************************/
  12. @Slf4j
  13. public class Quaternion {
  14.      
  15.     public float w;
  16.     public float x;
  17.     public float y;
  18.     public float z;
  19.  
  20.     public Quaternion() {
  21.      
  22.     }
  23.  
  24.     public Quaternion(Quaternion b) {
  25.      
  26.         this.w = b.w;
  27.         this.x = b.x;
  28.         this.y = b.y;
  29.         this.z = b.z;
  30.     }
  31.  
  32.     public Quaternion(float w, float x, float y, float z) {
  33.      
  34.         this.w = w;
  35.         this.x = x;
  36.         this.y = y;
  37.         this.z = z;
  38.     }
  39.  
  40.     //向量旋转
  41.     static void VectorRotation(float[] vector, Quaternion q) {
  42.      
  43.         Quaternion qv = new Quaternion(0, vector[0], vector[1], vector[2]);
  44.         //四元数旋转公式q0*qv*（q0逆）s
  45.         qv = Quaternion.Multiplication(Quaternion.Multiplication(q, qv), q.Inverse());
  46.         vector[0] = qv.x;
  47.         vector[1] = qv.y;
  48.         vector[2] = qv.z;
  49.     }
  50.  
  51.  
  52.     //返回欧拉角
  53.     public EulerAngles toEulerAngles() {
  54.      
  55.         // roll (x-axis rotation)
  56.         return new EulerAngles(this.w, this.x, this.y, this.z);
  57.     }
  58.  
  59.     //四元数相乘
  60.     static Quaternion Multiplication(Quaternion q0, Quaternion q1) {
  61.      
  62.         Quaternion ret = new Quaternion();
  63.         ret.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
  64.         ret.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
  65.         ret.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
  66.         ret.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
  67.         return ret;
  68.     }
  69.  
  70.     //四元数求逆
  71.     public Quaternion Inverse() {
  72.      
  73.         Quaternion ret;
  74.         ret = this;
  75.         ret.x *= -1;
  76.         ret.y *= -1;
  77.         ret.z *= -1;
  78.         return ret;
  79.     }
  80. }

2.3 欧拉角与四元数互转及球面线性插值算法

  1. package test;
  2.  
  3. import test.EulerAngles;
  4. import test.Quaternion;
  5. import lombok.extern.slf4j.Slf4j;
  6.  
  7. import static java.lang.Math.abs;
  8.  
  9. /*************************************
  10.  *Class Name: EulerAngle2QuatUtil
  11.  *Description: <四元数与欧拉角互转>
  12.  *@author: seminar
  13.  *@create: 2021/5/24
  14.  *@since 1.0.0
  15.  *************************************/
  16. @Slf4j
  17. public class EulerAngle2QuatUtil {
  18.      
  19.  
  20.     /**
  21.      * 归一化
  22.      *
  23.      * @param x
  24.      * @param y
  25.      * @param z
  26.      * @param w
  27.      * @return
  28.      */
  29.     public Quaternion normalizeQuaternion(float w, float x, float y, float z) {
  30.      
  31.         double lengthD = 1.0f / (w * w + x * x + y * y + z * z);
  32.         w *= lengthD;
  33.         x *= lengthD;
  34.         y *= lengthD;
  35.         z *= lengthD;
  36.         return new Quaternion(w, x, y, z);
  37.     }
  38.  
  39.     /**
  40.      * Slerp球面线性插值（Spherical Linear Interpolation）
  41.      *
  42.      * @param a 原始数据a
  43.      * @param b 原始数据b
  44.      * @param t 要插值的比例（中间插一个值1/2）
  45.      * @return
  46.      */
  47.     public Quaternion makeInterpolated(Quaternion a, Quaternion b, double t) {
  48.      
  49.         Quaternion out = new Quaternion();
  50.         double cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
  51.         if (cosHalfTheta < 0.0F) {
  52.      
  53.             b = new Quaternion(b);
  54.             cosHalfTheta = -cosHalfTheta;
  55.             b.x = -b.x;
  56.             b.y = -b.y;
  57.             b.z = -b.z;
  58.             b.w = -b.w;
  59.         }
  60.  
  61.         double halfTheta = (double) Math.acos((double) cosHalfTheta);
  62.         double sinHalfTheta = (double) Math.sqrt((double) (1.0F - cosHalfTheta * cosHalfTheta));
  63.         double ratioA;
  64.         double ratioB;
  65.         if ((double) abs(sinHalfTheta) > 0.001D) {
  66.      
  67.             double oneOverSinHalfTheta = 1.0F / sinHalfTheta;
  68.             ratioA = (double) Math.sin((double) ((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
  69.             ratioB = (double) Math.sin((double) (t * halfTheta)) * oneOverSinHalfTheta;
  70.         } else {
  71.      
  72.             ratioA = 1.0F - t;
  73.             ratioB = t;
  74.         }
  75.  
  76.         out.x = (float) (ratioA * a.x + ratioB * b.x);
  77.         out.y = (float) (ratioA * a.y + ratioB * b.y);
  78.         out.z = (float) (ratioA * a.z + ratioB * b.z);
  79.         out.w = (float) (ratioA * a.w + ratioB * b.w);
  80.  
  81.         out = normalizeQuaternion(out.w, out.x, out.y, out.z);
  82.         return out;
  83.     }
  84.  
  85.     /**
  86.      * 欧拉角（弧度）转四元数
  87.      *
  88.      * @param pitch
  89.      * @param yaw
  90.      * @param roll
  91.      * @return
  92.      */
  93.     public Quaternion toQuaternion(double pitch, double yaw, double roll) {
  94.      
  95.         EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll)); // 角度转弧度
  96.         return eu.toQuaternion();
  97.     }
  98.  
  99.     /**
  100.      * 四元数转欧拉角（弧度）
  101.      *
  102.      * @param quaternion
  103.      * @return
  104.      */
  105.     public EulerAngles toEulerAngles(Quaternion quaternion) {
  106.      
  107.         return quaternion.toEulerAngles();
  108.     }
  109.  
  110.     /**
  111.      * 姿态角——即欧拉角转四元数，对俩个四元数进行球面插值，四元数转回欧拉角并返回
  112.      *
  113.      * @param pitch    位置一俯仰角 -180~180
  114.      * @param yaw      位置一航向角 0~360
  115.      * @param roll     位置一翻滚角 -180~180
  116.      * @param pitch1   位置二俯仰角 -180~180
  117.      * @param yaw1     位置二俯仰角 0~360°
  118.      * @param roll1    位置二翻滚角 -180~180
  119.      * @param t        位置一时间
  120.      * @param t1       位置二时间
  121.      * @param t_insert 要计算姿态角的位置对应时间
  122.      * @return
  123.      */
  124.     public EulerAngles slerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
  125.      
  126.         // 位置1 欧拉角转四元数
  127.         // 位置2 欧拉角转四元数
  128.         Quaternion p = toQuaternion(pitch, yaw, roll);
  129.         Quaternion q = toQuaternion(pitch1, yaw1, roll1);
  130.  
  131.         // 计算插入的scale
  132.         float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));
  133.  
  134.         // Slerp球面线性插值
  135.         Quaternion r = makeInterpolated(q, p, scale);
  136.  
  137.         // 四元数转欧拉角
  138.         EulerAngles eulerAngles = r.toEulerAngles();
  139.         return eulerAngles;
  140.     }
  141.  
  142.     public static void main(String[] args) {
  143.      
  144. //        示例，中间1615609866585L的插值不太对
  145. //                         Roll    Pitch    Heading
  146. // 1615609866544L         -0.9    -0.405   358.809
  147. // 1615609866585L         -0.942   -0.362  314.489
  148. // 1615609866625L         -0.956   -0.331  0.178
  149.  
  150. //        正确结果
  151. //                         Roll    Pitch    Heading
  152. // 1615609866544L         -0.9,    -0.405,  358.809
  153. // 1615609866585L         -0.929， -0.368， 359.502
  154. // 1615609866625L         -0.956,  -0.331,  0.178
  155.  
  156.         // 调用EulerAngle2QuatUtil实现姿态角插值的获取
  157.         float roll = -0.9f, pitch = -0.405f, yaw = 358.809f;
  158.         EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
  159.         Quaternion p = eq.toQuaternion(pitch, yaw, roll);
  160.         log.info("p: {} {} {} {}", p.w, p.x, p.y, p.z);
  161.  
  162.         float roll1 = -0.956f, pitch1 = -0.331f, yaw1 = 0.178f;
  163.         Quaternion q = eq.toQuaternion(pitch1, yaw1, roll1);
  164.         log.info("q: {} {} {} {}", q.w, q.x, q.y, q.z);
  165.  
  166.         long t = 1615609866544L;
  167.         long t1 = 1615609866625L;
  168.         long t_insert = 1615609866585L;
  169.         float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));
  170.  
  171.         // Slerp球面线性插值
  172.         Quaternion r = eq.makeInterpolated(q, p, scale);
  173.         EulerAngles eulerAngles = r.toEulerAngles();
  174.         float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
  175.         float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
  176.         float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)
  177.  
  178.         log.info("{} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
  179.  
  180.         testSlerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);
  181.  
  182. //  0.000     -8.523      0.000
  183. //  0.000     -0.432     93.112
  184.         testSlerpInsert(-8.523f, 0.00f, 0.00f, -0.432f, 93.112f, 0.00f, t, t1, t_insert);
  185. //        0.000      1.054     66.847
  186. //        1.237     -1.956     62.336
  187.         testSlerpInsert(1.054f, 66.847f, 0.00f, -1.956f, 62.336f, 1.237f, t, t1, t_insert);
  188.  
  189. //        0.411      5.393    338.058
  190. //        0.402      5.395    338.063
  191.         testSlerpInsert(5.393f, 338.058f, 0.411f, 5.395f, 338.063f, 0.402f, t, t1, t_insert);
  192.     }
  193.  
  194.     private static void testSlerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
  195.      
  196.         log.info("==================testSlerpInsert start===============");
  197.         EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
  198.         EulerAngles eulerAngles = eq.slerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);
  199.         float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
  200.         float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
  201.         float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)
  202.  
  203.         log.info("slerpInsert {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
  204.         log.info("==================testSlerpInsert end=================");
  205.     }
  206.  
  207.     private static Quaternion getQuaternion(float roll, float pitch, float yaw) {
  208.      
  209.         EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
  210.         EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll));
  211.         Quaternion quaternion = eu.toQuaternion();
  212.         EulerAngles eulerAngles = quaternion.toEulerAngles();
  213.         float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
  214.         float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
  215.         float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)
  216.         log.info("toDegree: {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
  217.         return quaternion;
  218.     }
  219. }

参考

  • https://blog.csdn.net/xiaoma_bk/article/details/79082629?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2aggregatepagefirst_rank_v2~rank_aggregation-6-79082629.pc_agg_rank_aggregation&utm_term=%E5%9B%9B%E5%85%83%E6%95%B0%E6%AC%A7%E6%8B%89%E8%A7%92%E8%BD%AC%E6%8D%A2%E5%85%AC%E5%BC%8F&spm=1000.2123.3001.4430
  • 在线转换工具
  • 四元数插值
  • 四元数插值2
  • 四元数与欧拉角互转

转载自：欧拉角与四元数互转，及四元数slerp球面线性插值算法 - it610.com

本文内容由网友自发贡献，转载请注明出处：https://www.wpsshop.cn/w/不正经/article/detail/422751
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号