歐拉角與四元數互轉，及四元數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 歐拉角類

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 四元數類

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逆）sqv = 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 歐拉角與四元數互轉及球面線性插值算法

球面線性插值也稱四元數內插，更加光滑；

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);// 計算插入的scalefloat 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.112testSlerpInsert(-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.336testSlerpInsert(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.063testSlerpInsert(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-2_{aggregatepage}first_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球面线性插值算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。