自用的四元数、欧拉角、旋转矩阵转换代码

C++

// 四元数转旋转矩阵
Mat Quaternion_to_Rotation(Mat Q) {
    double q0 = Q.at(0, 0), q1 = Q.at(1, 0);
    double q2 = Q.at(2, 0), q3 = Q.at(3, 0);
    return Mat(3, 3, vecdble{
        q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,
        2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,
        2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
    });
}
// 四元数乘法
Mat Quaternion_Product(Mat Q1, Mat Q2) {
    double s1 = Q1.at(0, 0);
    double s2 = Q2.at(0, 0);
    Vector3d v1(Q1.at(1, 0), Q1.at(2, 0), Q1.at(3, 0));
    Vector3d v2(Q2.at(1, 0), Q2.at(2, 0), Q2.at(3, 0));
    double s = s1*s2 - v1*v2;
    Vector3d v = s1*v2 + s2*v1 + (v1 & v2);
    return Mat(4, 1, vecdble{s, v._x, v._y, v._z});
}
// 四元数求逆
Mat Quaternion_Inverse(Mat Q) {
    return Mat(4, 1, vecdble{Q.at(0, 0), -Q.at(1, 0), -Q.at(2, 0), -Q.at(3, 0)});
}
// 四元数转欧拉角
Mat Quaternion_to_Euler(Mat Q) {
    double q0 = Q.at(0, 0);
    double q1 = Q.at(1, 0);
    double q2 = Q.at(2, 0);
    double q3 = Q.at(3, 0);
    double rol=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	double pit=asin(LIMIT(2*(q0*q2-q1*q3), -1, 1));
	double yaw=atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));
    return Mat(3, 1, vecdble{rol, pit, yaw});
}
// 四元数长度
double Quaternion_Norm2(Mat Q) {
    double sumq = 0;
    for (uint32_t i = 0; i < 4; i++)
        sumq += Q.at(i, 0)*Q.at(i, 0);
    return sumq;
}
// 四元数归一化
Mat Quaternion_Normalize(Mat Q) {
    double sumq = 0;
    for (uint32_t i = 0; i < 4; i++)
        sumq += Q.at(i, 0)*Q.at(i, 0);
    return Q / sqrt(sumq);
}
// 欧拉角转四元数
Mat Euler_To_Quaternion(Mat euler) {
    double cy = cos(euler.at(2,0) * 0.5);
    double sy = sin(euler.at(2,0) * 0.5);
    double cp = cos(euler.at(1,0) * 0.5);
    double sp = sin(euler.at(1,0) * 0.5);
    double cr = cos(euler.at(0,0) * 0.5);
    double sr = sin(euler.at(0,0) * 0.5);
    double q0 = cy * cp * cr + sy * sp * sr;
    double q1 = cy * cp * sr - sy * sp * cr;
    double q2 = sy * cp * sr + cy * sp * cr;
    double q3 = sy * cp * cr - cy * sp * sr;
    return Mat(vecdble{q0, q1, q2, q3});
}
// 控制律
Mat Control_Law(Mat eulerTarget, Mat Qcurrent, Mat omega) {
    Mat torque;  // 控制律输出
    Mat Qtarget = Euler_To_Quaternion(eulerTarget);
    Mat Qerr = Quaternion_Product(Qtarget, Quaternion_Inverse(Qcurrent));
    if (Qerr.at(0, 0) < 0)
        Qerr = Mat(4, 1) - Qerr;
    if (Qerr.at(0, 0) < 0.6) {  // 大角度误差
        torque = Mat(3, 1, vecdble{Qerr.at(1, 0), Qerr.at(2, 0), Qerr.at(3, 0)});
        torque = Mat(3, 1) - torque*data1.at(9,0) - omega*data1.at(10,0);
    } else {  // 小角度误差
        Mat eulerCurrent = Quaternion_to_Euler(Qcurrent);
        Mat eulerError = eulerTarget - eulerCurrent;
        torque = eulerError*data1.at(11,0);
        torque -= omega*data1.at(12,0);
    }
    return torque;
}

int main(void) {
    Mat q1 = Euler_To_Quaternion(Mat(vecdble{0.5, -1, 0}));
    Mat q2 = Euler_To_Quaternion(Mat(vecdble{1.2, -0.5, 0.2}));
    Mat q3 = Euler_To_Quaternion(Mat(vecdble{-0.1, 0.2, -0.3}));
    Mat ansq1 = Quaternion_Product(Quaternion_Product(q1, q2), q3);
    Mat ansq2 = Quaternion_Product(q1, Quaternion_Product(q2, q3));
    cout << Quaternion_Product(q1, q2) << endl;
    cout << Quaternion_Product(q2, q1) << endl;
    // cout.precision(16);
    // cout << Quaternion_Norm2(ansq2) << endl;
}

python

import numpy as np
def Quaternion_to_Rotation(Q):
    q0, q1, q2, q3 = Q
    return np.array([
        [q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2],
        [2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1],
        [2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3],
    ])

def Quaternion_Product(Q1, Q2):
    s1 = Q1[0]
    s2 = Q2[0]
    v1 = np.array([Q1[1], Q1[2], Q1[3]])
    v2 = np.array([Q2[1], Q2[2], Q2[3]])
    s = s1*s2 - np.dot(v1, v2)
    v = s1*v2 + s2*v1 + np.cross(v1, v2)
    return np.array([s, v[0], v[1], v[2]])

def Quaternion_Inverse(Q):
    return np.array([Q[0], -Q[1], -Q[2], -Q[3]])

def Euler_To_Quaternion(euler):
    cy = np.cos(euler[2] * 0.5)
    sy = np.sin(euler[2] * 0.5)
    cp = np.cos(euler[1] * 0.5)
    sp = np.sin(euler[1] * 0.5)
    cr = np.cos(euler[0] * 0.5)
    sr = np.sin(euler[0] * 0.5)
    q0 = cy * cp * cr + sy * sp * sr
    q1 = cy * cp * sr - sy * sp * cr
    q2 = sy * cp * sr + cy * sp * cr
    q3 = sy * cp * cr - cy * sp * sr
    return np.array([q0, q1, q2, q3])

q1 = Euler_To_Quaternion(np.array([0.5, -1, 0]))
q2 = Euler_To_Quaternion(np.array([1.2, -0.5, 0.2]))
q3 = Euler_To_Quaternion(np.array([-0.1, 0.2, -0.3]))
print(Quaternion_Product(q1, q2))
print(Quaternion_Product(q2, q1))