不同角度范围下四元数转欧拉角的方式

前言

在标定过程中求出的欧拉角与预设真值差距太大，检查中发现求出的角度与真值角度都可以将车辆坐标系变换到相机坐标系。后通过查阅文献，发现四元数对应的欧拉角并不唯一，在不同的条件下可求出不同的欧拉角，实际应用中需根据实际设定的角度范围选择对应的欧拉角。

[1] 夏喜旺,杜涵,刘汉兵.关于大角度范围内四元数与欧拉角转换的思考[J].导弹与航天运载技术,2012,(05):47-53.

四元数与欧拉角

使用欧拉角描述空间旋转时， θ \theta θ与 2 π + θ 2\pi+\theta 2π+θ可表示相同的姿态，即使在 ( − π , π ) (-\pi,\pi) (−π,π)范围内，也有 ( ψ , θ , γ ) (\psi,\theta,\gamma) (ψ,θ,γ)与 ( ψ − s g n ( ψ ) π , s g n ( θ ) π − θ , γ − s g n ( γ ) π ) (\psi-sgn(\psi)\pi,sgn(\theta)\pi-\theta,\gamma-sgn(\gamma)\pi) (ψ−sgn(ψ)π,sgn(θ)π−θ,γ−sgn(γ)π)表示相同的姿态。

四元数描述姿态

设 ( ψ , θ , γ ) (\psi,\theta,\gamma) (ψ,θ,γ)为z-y-x旋转顺序下的欧拉角，则当前旋转对应的欧拉角为：
在这里插入图片描述
上式也可看作欧拉角到四元数的映射关系。
四元数对应的旋转矩阵为：
在这里插入图片描述上式带入四元数各项的值，可得到下式：
在这里插入图片描述

四元数转欧拉角

设 θ ∈ ( − π , π ) \theta\in(-\pi,\pi) θ∈(−π,π)，对应不同取值范围下四元数转欧拉角的方式如下。

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

代码

// 四元数转欧拉角
void quatToEuler(const Vec4d& quat, Vec3d& euler){
    double qw, qx, qy, qz, roll, pitch, yaw, rad2deg;
    qw = quat[0];
    qx = quat[1];
    qy = quat[2];
    qz = quat[3];
    rad2deg = 180.0 / M_PI;

    // [1]夏喜旺,杜涵,刘汉兵.关于大角度范围内四元数与欧拉角转换的思考[J].导弹与航天运载技术,2012,(05):47-53.
    // 1. pitch ∈ (-π/2，π/2)
    // roll = atan2(2 * (qw * qx + qy * qz), 2 * (pow(qw,2) + pow(qz,2)) - 1) * rad2deg;
    // pitch = asin(2 * (qw * qy - qz * qx)) * rad2deg;
    // yaw = atan2(2 * (qw * qz + qx * qy), 2 * (pow(qw,2) + pow(qx,2)) - 1) * rad2deg;
    
    // 2. pitch ∈ (-π，-π/2)∪(π/2，π)
    roll = atan2(- 2 * (qw * qx + qy * qz), 1 - 2 * (pow(qw,2) + pow(qz,2))) * rad2deg;
    pitch = asin(2 * (qw * qy - qz * qx)) * rad2deg;
    yaw = atan2(- 2 * (qw * qz + qx * qy), 1 - 2 * (pow(qw,2) + pow(qx,2))) * rad2deg;

    // 3. pitch = (+-2/π)
    // 有无穷多组欧拉角组合与四元数对应，只能确定roll+-yaw的值

    // 1与2情形下的欧拉角可直接转换
    // roll = roll - roll/abs(roll) * 180;
    // pitch = pitch/abs(pitch) * 180 - pitch;
    // yaw = yaw - yaw/abs(yaw) * 180;

    euler = {roll, pitch, yaw};

    cout << "eurler: " << euler << endl;
}