1. 首页
  2. 文章中心
  3. JAVA

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

81 阅读 0 评论
我是靠谱客的博主 阔达小兔子，这篇文章主要介绍欧拉角与四元数互转，及四元数slerp球面线性插值算法，现在分享给大家，希望可以做个参考。

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

    • 1. 欧拉角与四元数是什么？
    • 2. 源码
      • 2.1 欧拉角类
      • 2.2 四元数类
      • 2.3 欧拉角与四元数互转及球面线性插值算法
    • 参考

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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
package test;

/****************************
 * Class Name： EulerAngles
 * Description： <欧拉角类>
 * @Author: seminar
 * @create: 2021/05/21
 * @since: 1.0.0
 ***************************/
public class EulerAngles {
    /**
     * Math.toRadians(roll) 角度转弧度
     * Math.toDegrees(roll) 弧度转角度
     * <p>
     * 翻滚角（roll） 弧度
     */
    public double roll;

    /**
     * 俯仰角(pitch) 弧度
     */
    public double pitch;

    /**
     * yaw 即heading(航向角) 弧度
     */
    public double yaw;

    public EulerAngles(float pitch, float yaw, float roll) {
        this.pitch = pitch;
        this.yaw = yaw;
        this.roll = roll;
    }

    public EulerAngles(float w, float x, float y, float z) {
        // roll (x-axis rotation)
        float sinr_cosp = 2 * (w * x + y * z);
        float cosr_cosp = 1 - 2 * (x * x + y * y);
        this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);

        // pitch (y-axis rotation)
        float sinp = 2 * (w * y - z * x);
        if (Math.abs(sinp) >= 1) {
            this.pitch = Math.copySign(1.57075f, sinp); // use 90 degrees if out of range
        } else {
            this.pitch = (float) Math.asin(sinp);
        }

        // yaw (z-axis rotation)
        float siny_cosp = 2 * (w * z + x * y);
        float cosy_cosp = 1 - 2 * (y * y + z * z);
        this.yaw = (float) Math.atan2(siny_cosp, cosy_cosp);
    }

    public Quaternion toQuaternion() {
        //欧拉角转四元数，角度减半是因为四元数旋转计算时需要旋转两次，具体原理请查看四元数原理
        float cy = (float) Math.cos(yaw * 0.5f);
        float sy = (float) Math.sin(yaw * 0.5f);
        float cp = (float) Math.cos(pitch * 0.5f);
        float sp = (float) Math.sin(pitch * 0.5f);
        float cr = (float) Math.cos(roll * 0.5f);
        float sr = (float) Math.sin(roll * 0.5f);
        Quaternion q = new Quaternion();
        q.w = cy * cp * cr + sy * sp * sr;
        q.x = cy * cp * sr - sy * sp * cr;
        q.y = sy * cp * sr + cy * sp * cr;
        q.z = sy * cp * cr - cy * sp * sr;
        return q;
    }
}

2.2 四元数类

复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
package test;

import lombok.extern.slf4j.Slf4j;

/****************************
 * Class Name： Quaternion
 * Description： <四元数类>
 * @Author: seminar
 * @create: 2021/05/21
 * @since: 1.0.0
 ***************************/
@Slf4j
public class Quaternion {
    public float w;
    public float x;
    public float y;
    public float z;

    public Quaternion() {
    }

    public Quaternion(Quaternion b) {
        this.w = b.w;
        this.x = b.x;
        this.y = b.y;
        this.z = b.z;
    }

    public Quaternion(float w, float x, float y, float z) {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }

    //向量旋转
    static void VectorRotation(float[] vector, Quaternion q) {
        Quaternion qv = new Quaternion(0, vector[0], vector[1], vector[2]);
        //四元数旋转公式q0*qv*（q0逆）s
        qv = Quaternion.Multiplication(Quaternion.Multiplication(q, qv), q.Inverse());
        vector[0] = qv.x;
        vector[1] = qv.y;
        vector[2] = qv.z;
    }


    //返回欧拉角
    public EulerAngles toEulerAngles() {
        // roll (x-axis rotation)
        return new EulerAngles(this.w, this.x, this.y, this.z);
    }

    //四元数相乘
    static Quaternion Multiplication(Quaternion q0, Quaternion q1) {
        Quaternion ret = new Quaternion();
        ret.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
        ret.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
        ret.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
        ret.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
        return ret;
    }

    //四元数求逆
    public Quaternion Inverse() {
        Quaternion ret;
        ret = this;
        ret.x *= -1;
        ret.y *= -1;
        ret.z *= -1;
        return ret;
    }
}

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

球面线性插值也称四元数内插，更加光滑；

复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
package test;

import test.EulerAngles;
import test.Quaternion;
import lombok.extern.slf4j.Slf4j;

import static java.lang.Math.abs;

/*************************************
 *Class Name: EulerAngle2QuatUtil
 *Description: <四元数与欧拉角互转>
 *@author: seminar
 *@create: 2021/5/24
 *@since 1.0.0
 *************************************/
@Slf4j
public class EulerAngle2QuatUtil {

    /**
     * 归一化
     *
     * @param x
     * @param y
     * @param z
     * @param w
     * @return
     */
    public Quaternion normalizeQuaternion(float w, float x, float y, float z) {
        double lengthD = 1.0f / (w * w + x * x + y * y + z * z);
        w *= lengthD;
        x *= lengthD;
        y *= lengthD;
        z *= lengthD;
        return new Quaternion(w, x, y, z);
    }

    /**
     * Slerp球面线性插值（Spherical Linear Interpolation）
     *
     * @param a 原始数据a
     * @param b 原始数据b
     * @param t 要插值的比例（中间插一个值1/2）
     * @return
     */
    public Quaternion makeInterpolated(Quaternion a, Quaternion b, double t) {
        Quaternion out = new Quaternion();
        double cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
        if (cosHalfTheta < 0.0F) {
            b = new Quaternion(b);
            cosHalfTheta = -cosHalfTheta;
            b.x = -b.x;
            b.y = -b.y;
            b.z = -b.z;
            b.w = -b.w;
        }

        double halfTheta = (double) Math.acos((double) cosHalfTheta);
        double sinHalfTheta = (double) Math.sqrt((double) (1.0F - cosHalfTheta * cosHalfTheta));
        double ratioA;
        double ratioB;
        if ((double) abs(sinHalfTheta) > 0.001D) {
            double oneOverSinHalfTheta = 1.0F / sinHalfTheta;
            ratioA = (double) Math.sin((double) ((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
            ratioB = (double) Math.sin((double) (t * halfTheta)) * oneOverSinHalfTheta;
        } else {
            ratioA = 1.0F - t;
            ratioB = t;
        }

        out.x = (float) (ratioA * a.x + ratioB * b.x);
        out.y = (float) (ratioA * a.y + ratioB * b.y);
        out.z = (float) (ratioA * a.z + ratioB * b.z);
        out.w = (float) (ratioA * a.w + ratioB * b.w);

        out = normalizeQuaternion(out.w, out.x, out.y, out.z);
        return out;
    }

    /**
     * 欧拉角（弧度）转四元数
     *
     * @param pitch
     * @param yaw
     * @param roll
     * @return
     */
    public Quaternion toQuaternion(double pitch, double yaw, double roll) {
        EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll)); // 角度转弧度
        return eu.toQuaternion();
    }

    /**
     * 四元数转欧拉角（弧度）
     *
     * @param quaternion
     * @return
     */
    public EulerAngles toEulerAngles(Quaternion quaternion) {
        return quaternion.toEulerAngles();
    }

    /**
     * 姿态角——即欧拉角转四元数，对俩个四元数进行球面插值，四元数转回欧拉角并返回
     *
     * @param pitch    位置一俯仰角 -180~180
     * @param yaw      位置一航向角 0~360
     * @param roll     位置一翻滚角 -180~180
     * @param pitch1   位置二俯仰角 -180~180
     * @param yaw1     位置二俯仰角 0~360°
     * @param roll1    位置二翻滚角 -180~180
     * @param t        位置一时间
     * @param t1       位置二时间
     * @param t_insert 要计算姿态角的位置对应时间
     * @return
     */
    public EulerAngles slerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
        // 位置1 欧拉角转四元数
        // 位置2 欧拉角转四元数
        Quaternion p = toQuaternion(pitch, yaw, roll);
        Quaternion q = toQuaternion(pitch1, yaw1, roll1);

        // 计算插入的scale
        float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));

        // Slerp球面线性插值
        Quaternion r = makeInterpolated(q, p, scale);

        // 四元数转欧拉角
        EulerAngles eulerAngles = r.toEulerAngles();
        return eulerAngles;
    }

    public static void main(String[] args) {
//        示例，中间1615609866585L的插值不太对
//                         Roll    Pitch    Heading
// 1615609866544L         -0.9    -0.405   358.809
// 1615609866585L         -0.942   -0.362  314.489
// 1615609866625L         -0.956   -0.331  0.178

//        正确结果
//                         Roll    Pitch    Heading
// 1615609866544L         -0.9,    -0.405,  358.809
// 1615609866585L         -0.929， -0.368， 359.502
// 1615609866625L         -0.956,  -0.331,  0.178

        // 调用EulerAngle2QuatUtil实现姿态角插值的获取
        float roll = -0.9f, pitch = -0.405f, yaw = 358.809f;
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        Quaternion p = eq.toQuaternion(pitch, yaw, roll);
        log.info("p: {} {} {} {}", p.w, p.x, p.y, p.z);

        float roll1 = -0.956f, pitch1 = -0.331f, yaw1 = 0.178f;
        Quaternion q = eq.toQuaternion(pitch1, yaw1, roll1);
        log.info("q: {} {} {} {}", q.w, q.x, q.y, q.z);

        long t = 1615609866544L;
        long t1 = 1615609866625L;
        long t_insert = 1615609866585L;
        float scale = (float) ((t_insert - t) / ((t1 - t) * 1.0));

        // Slerp球面线性插值
        Quaternion r = eq.makeInterpolated(q, p, scale);
        EulerAngles eulerAngles = r.toEulerAngles();
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)

        log.info("{} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));

        testSlerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);

//  0.000     -8.523      0.000
//  0.000     -0.432     93.112
        testSlerpInsert(-8.523f, 0.00f, 0.00f, -0.432f, 93.112f, 0.00f, t, t1, t_insert);
//        0.000      1.054     66.847
//        1.237     -1.956     62.336
        testSlerpInsert(1.054f, 66.847f, 0.00f, -1.956f, 62.336f, 1.237f, t, t1, t_insert);

//        0.411      5.393    338.058
//        0.402      5.395    338.063
        testSlerpInsert(5.393f, 338.058f, 0.411f, 5.395f, 338.063f, 0.402f, t, t1, t_insert);
    }

    private static void testSlerpInsert(float pitch, float yaw, float roll, float pitch1, float yaw1, float roll1, long t, long t1, long t_insert) {
        log.info("==================testSlerpInsert start===============");
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        EulerAngles eulerAngles = eq.slerpInsert(pitch, yaw, roll, pitch1, yaw1, roll1, t, t1, t_insert);
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)

        log.info("slerpInsert {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
        log.info("==================testSlerpInsert end=================");
    }

    private static Quaternion getQuaternion(float roll, float pitch, float yaw) {
        EulerAngle2QuatUtil eq = new EulerAngle2QuatUtil();
        EulerAngles eu = new EulerAngles((float) Math.toRadians(pitch), (float) Math.toRadians(yaw), (float) Math.toRadians(roll));
        Quaternion quaternion = eu.toQuaternion();
        EulerAngles eulerAngles = quaternion.toEulerAngles();
        float roll2 = (float) Math.toDegrees(eulerAngles.roll); // 弧度转回角度
        float pitch2 = (float) Math.toDegrees(eulerAngles.pitch); // 弧度转回角度
        float heading2 = (float) (Math.toDegrees(eulerAngles.yaw) > 0 ? Math.toDegrees(eulerAngles.yaw) : Math.toDegrees(eulerAngles.yaw) + 360); // 弧度转回角度(航向角0~360°)
        log.info("toDegree: {} {} {}", Double.parseDouble(String.format("%.3f", roll2)), Double.parseDouble(String.format("%.3f", pitch2)), Double.parseDouble(String.format("%.3f", heading2)));
        return quaternion;
    }
}

参考

  • 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球面线性插值算法的全部内容，更多相关欧拉角与四元数互转内容请搜索靠谱客的其他文章。

本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
点赞(54)

相关文章

ROS中为什么用四元数描述运动控制
ROS中为什么用四元数描述运动控制
第三部分 单机以及编队控制实验—中级教程之传感器数据读取实验（1）
第三部分 单机以及编队控制实验—中级教程之传感器数据读取实验（1）
四元数与欧拉角（RPY角）的相互转换
四元数与欧拉角（RPY角）的相互转换
android室内定位传感器辅助pdr jar,基于PDR辅助的视觉室内定位算法的研究
android室内定位传感器辅助pdr jar,基于PDR辅助的视觉室内定位算法的研究
欧拉角与四元数互转，及四元数slerp球面线性插值算法
欧拉角与四元数互转，及四元数slerp球面线性插值算法
[CAD二次开发]获取CAD内3D块参照的欧拉旋转角，Matrix3d矩阵转欧拉角。
[CAD二次开发]获取CAD内3D块参照的欧拉旋转角，Matrix3d矩阵转欧拉角。
经纬度计算距离公式_获得中国大陆各地市间距离
经纬度计算距离公式_获得中国大陆各地市间距离
根据经纬度计算两点距离和航向
根据经纬度计算两点距离和航向

评论列表共有 0 条评论

立即
投稿
返回
顶部