MPU6050欧拉角计算旋转后三维坐标平面与水平面(初始xoy平面)夹角算法（外旋）

TA很懒，还没有添加名字

已于 2023-08-01 16:49:32 修改

阅读量2.3k

点赞数

分类专栏： Arduion 文章标签： python c语言 c++ 算法

于 2023-06-08 11:35:06 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_49861340/article/details/131093279

版权

Arduion 专栏收录该内容

4 篇文章

订阅专栏

本文详细介绍了MPU6050传感器在加速度和角速度测量基础上，结合欧拉角进行旋转计算的过程，包括外旋和内旋的区别，以及可能出现的万向节死锁问题。同时，提供了C++和Python的旋转算法实现，用于计算旋转后的坐标平面与初始平面的夹角。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

前言

MPU6050六轴传感器可通过加速度传感器和陀螺仪同时输出加速度和角速度，配合一些开源项目（fusion、DMP等）计算出相应的四元素和欧拉角等，可通过欧拉角计算出旋转后的三维坐标系的各平面（旋转后得到的xoy平面、xoz平面、yoz平面）与水平面（初始xoy平面）夹角。

欧拉角旋转定义（重要）

在使用欧拉角演示旋转之前，必须先规定欧拉角旋转顺序！！！（z->y->x or z->x->y等顺序）因为在围绕x轴、y轴、z轴旋转相同的角度，顺序不一样最后得到的旋转姿态会有所不同。大部分开源旋转顺序计算使用的是z-y-x顺序旋转包括MPU6050自带的DMP融合算法。

欧拉角外旋

欧拉角外旋通俗讲就是使用世界坐标系，在一些开源项目里使用加速度和角速度计算出的欧拉角，无论MPU6050处于什么样的状态上电，x轴和y轴组成的平面始终平行于水平面。

（重要！！！）在外旋的旋转过程中，因为已经规定了旋转顺序，正常来说就必须按照旋转轴的顺序进行旋转。在正常的外旋情况下，排在前面的旋转轴先进行旋转时，不会影响排在后面顺序的轴初始状态的改变（因为世界坐标的缘故）。但是如果排在后面的旋转轴先旋转了，再旋转排在前面的旋转轴，就会导致前面旋转轴初始方向的改变。

欧拉角内旋

欧拉角内旋就是使用本地坐标系，即初始三维坐标的x轴和y轴组成的平面就是MPU6050芯片上电时所处的平面。

（重要！！！）在内旋的旋转过程中，因为已经规定了旋转顺序，正常来说就必须按照旋转轴的顺序进行旋转。在正常的内旋情况下，排在前面的旋转轴先进行旋转时，会影响排在后面顺序的轴初始状态的改变（后面的旋转轴会绕着排在前面的旋转轴旋转相同的旋转角度，并改变初始方向，后面的旋转轴根据前面旋转后得到的新的旋转轴再进行旋转）。如果排在后面的旋转轴先旋转了，再旋转排在前面的旋转轴，也会导致前面旋转轴初始方向的改变。

万向节死锁（重要！！）

内旋：当我规定了以z-y-x顺序旋转时。如果y轴先旋转，并且刚好旋转了90°，（因为后面的旋转会影响前面轴初始态的缘故）就会导致z轴和x轴重合且方向相同，我再进行z轴或者x轴旋转，欧拉角x和z就会同时递增或者递减；如果y轴先旋转，并且刚好旋转了-90°，（因为后面的旋转会影响前面轴初始态的缘故）就会导致z轴和x轴重合且方向相反，我再进行z轴或者x轴旋转，欧拉角x和z就会同时一个递增一个递减；

外旋：当我规定了以z-y-x顺序旋转时。如果y轴先旋转，并且刚好旋转了-90°，（因为后面的旋转会影响前面轴初始态的缘故）就会导致z轴和x轴重合且方向相同，我再进行z轴或者x轴旋转，欧拉角x和z就会同时递增或者递减；如果y轴先旋转，并且刚好旋转了90°，（因为后面的旋转会影响前面轴初始态的缘故）就会导致z轴和x轴重合且方向相反，我再进行z轴或者x轴旋转，欧拉角x和z就会同时一个递增一个递减；

同理：规定了其他旋转顺序进行旋转时，只要在中间的旋转轴先旋转，并且刚好旋转了90°或-90°，就会发生万向节死锁。

关于欧拉角内旋和外旋问题可以通过Github上的开源项目rotation_master进行演示方便理解（感谢这位大神）

Release v20220112 · iwatake2222/rotation_master · GitHub

MPU6050缺陷

MPU6050在没有外部磁力传感器的作用下时，芯片就没法获取地磁数据（地球磁力），所以就会导致欧拉角z在不断改变（上电初始化后，在使用一段时间后回到初始状态欧拉角z会有莫名其妙的角度，并且内旋外旋都存在这个问题），欧拉角z不断改变的结果就是：在三维旋转坐标中，x轴和y轴的初始方向的改变。

算法衍生

虽然x轴和y轴的初始方向在不断改变，但他们所组成的xoy平面是不会改变的，所以在规定了一定旋转顺序旋转后的三维坐标各平面与初始xoy平面所构成的面面角就不会因为z轴（欧拉角z）的不确定性而发生改变，算法的构思适用于内旋和外旋，但是算法不同。

面面角取值范围为0°—180°，可根据欧拉角的正负号判定面面角的正负号，就能给出旋转角度为-180°—180°，从而就能得出所有旋转状态返回的三个面面角组成的数组唯一性。（这里只给出了C/C++的正负号判断，Python重写很简单，但是我懒😂）

C/C++代码（外旋）

// 绕某旋转后点及初始轴
double *RodriguesRotate(double angle, double x1, double y1, double z1, double x2, double y2, double z2){

    double sin_angle = sin(angle * M_PI / 180.0);
    double cos_angle = cos(angle * M_PI / 180.0);
  
    double x = x1 * cos_angle + (y2 * z1 - z2 * y1) * sin_angle + x2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    double y = y1 * cos_angle + (z2 * x1 - x2 * z1) * sin_angle + y2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    double z = z1 * cos_angle + (x2 * y1 - y2 * x1) * sin_angle + z2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    
    double *verts = new double [3];
    verts[0] = x;
    verts[1] = y;
    verts[2] = z;
    
    return verts;
}

// 过原点平面法向量计算
double *normal_vector(double x1,double y1,double z1,double x2,double y2,double z2){

    double a = (y1*z2)-(y2*z1);
    double b = (z1*x2)-(z2*x1);
    double c = (x1*y2)-(x2*y1);

    double *normal_vectors = new double [3];
    normal_vectors[0] = a;
    normal_vectors[1] = b;
    normal_vectors[2] = c;  
    
    return normal_vectors;
}

// 旋法向量与初始xoy平面法向量夹角
double dihedral_angle(double x, double y, double z){
    double angle = acos((x * 0 + y * 0 + z * 1) / sqrt(x * x + y * y + z * z)) * 180 / M_PI;
    return angle;
}



//主函数：
    
    Serial.print("欧拉角X : ");Serial.print(mpu6050.getAngleX());
    Serial.print("\t欧拉角Y : ");Serial.print(mpu6050.getAngleY());
    Serial.print("\t欧拉角Z : ");Serial.println(mpu6050.getAngleZ());

    // 定义初始点坐标
    double verts[4][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}};

    // 计算X轴，Y轴旋转后初始坐标
    double *Sz_Y = RodriguesRotate(mpu6050.getAngleZ(),verts[2][0], verts[2][1], verts[2][2], verts[3][0], verts[3][1], verts[3][2]);
    double *Sz_X = RodriguesRotate(mpu6050.getAngleZ(), verts[1][0], verts[1][1], verts[1][2], verts[3][0], verts[3][1], verts[3][2]);
    double *Szy_X = RodriguesRotate(mpu6050.getAngleY(), Sz_X[0], Sz_X[1], Sz_X[2], Sz_Y[0], Sz_Y[1], Sz_Y[2]);

    // 计算旋转后点坐标
    double new_o[3] = {verts[0][0], verts[0][1], verts[0][2]};
    
    double *x_rz = RodriguesRotate(mpu6050.getAngleZ(), verts[1][0], verts[1][1], verts[1][2], verts[3][0], verts[3][1], verts[3][2]);
    double *x_ry = RodriguesRotate(mpu6050.getAngleY(), x_rz[0],x_rz[1],x_rz[2],Sz_Y[0],Sz_Y[1],Sz_Y[2]);
    double *new_x = RodriguesRotate(mpu6050.getAngleX(), x_ry[0],x_ry[1],x_ry[2],Szy_X[0],Szy_X[1],Szy_X[2]);
    if(!x_rz) delete(x_rz);
    if(!x_ry) delete(x_ry);

    double *y_rz = RodriguesRotate(mpu6050.getAngleZ(), verts[2][0], verts[2][1], verts[2][2], verts[3][0], verts[3][1], verts[3][2]);
    double *y_ry = RodriguesRotate(mpu6050.getAngleY(), y_rz[0],y_rz[1],y_rz[2],Sz_Y[0],Sz_Y[1],Sz_Y[2]);
    double *new_y = RodriguesRotate(mpu6050.getAngleX(), y_ry[0],y_ry[1],y_ry[2],Szy_X[0],Szy_X[1],Szy_X[2]);
    if(!y_rz) delete(y_rz);
    if(!y_ry) delete(y_ry);

    double *z_rz = RodriguesRotate(mpu6050.getAngleZ(), verts[3][0], verts[3][1], verts[3][2], verts[3][0], verts[3][1], verts[3][2]);
    double *z_ry = RodriguesRotate(mpu6050.getAngleY(), z_rz[0],z_rz[1],z_rz[2],Sz_Y[0],Sz_Y[1],Sz_Y[2]);
    double *new_z = RodriguesRotate(mpu6050.getAngleX(), z_ry[0],z_ry[1],z_ry[2],Szy_X[0],Szy_X[1],Szy_X[2]);
    if(!z_rz) delete(z_rz);
    if(!z_ry) delete(z_ry);

    if(!Sz_Y) delete(Sz_Y);
    if(!Sz_X) delete(Sz_X);
    if(!Szy_X) delete(Szy_X);

    // 旋转后的三维坐标系的点组成的三个面的法向量
    double *normal_vector_xoy = normal_vector(new_x[0], new_x[1], new_x[2], new_y[0], new_y[1], new_y[2]);
    double *normal_vector_xoz = normal_vector(new_x[0], new_x[1], new_x[2], new_z[0], new_z[1], new_z[2]);
    double *normal_vector_yoz = normal_vector(new_y[0], new_y[1], new_y[2], new_z[0], new_z[1], new_z[2]);
    if(!new_x) delete(new_x);
    if(!new_y) delete(new_y);
    if(!new_z) delete(new_z);

    // 法向量与初始xoy平面法向量夹角
    double dihedral_angle_xoy = dihedral_angle(normal_vector_xoy[0], normal_vector_xoy[1], normal_vector_xoy[2]);
    double dihedral_angle_xoz = dihedral_angle(normal_vector_xoz[0], normal_vector_xoz[1], normal_vector_xoz[2]);
    double dihedral_angle_yoz = dihedral_angle(normal_vector_yoz[0], normal_vector_yoz[1], normal_vector_yoz[2]);

    //范围：-180°<——>+180°
    if (normal_vector_yoz[2] >= 0) {
      Serial.print("XY与水平面夹角:");
      Serial.print(dihedral_angle_xoy);
    }
    else{
      Serial.print("XY与水平面夹角:");
      Serial.print(- dihedral_angle_xoy);
    }
    if (normal_vector_xoy[2] >= 0) {
      Serial.print("XZ与水平面夹角:");
      Serial.print(dihedral_angle_xoz);
    }
    else {
      Serial.print("XZ与水平面夹角:");
      Serial.print(- dihedral_angle_xoz);
    }
    if (normal_vector_xoz[2] >= 0) {
      Serial.print("YZ与水平面夹角:");
      Serial.print(dihedral_angle_yoz);
    }
    else{
      Serial.print("YZ与水平面夹角:");
      Serial.print(- dihedral_angle_yoz);
    }

    if(!normal_vector_xoy) delete(normal_vector_xoy);
    if(!normal_vector_xoz) delete(normal_vector_xoz);
    if(!normal_vector_yoz) delete(normal_vector_yoz);

Python代码（外旋）（使用numpy）

import math
import numpy as np

# 计算绕某轴旋转后的点及顺序后的旋转轴
def RodriguesRotate(angle, v, u):
    '''向量v绕向量u旋转角度θ,得到新的向量vert
    罗德里格斯旋转公式:v' = vcosθ + (u×v)sinθ + (u·v)u(1-cosθ)

    args:
        v:向量,维度为(3,)
        u:作为旋转轴的向量,维度为(3,)
        angle:旋转角度θ,此处默认为角度值
    returns:
        vert:旋转后得到的向量,维度为(3,)
    '''
    u = u / np.linalg.norm(u)  # 计算单位向量
    sin_angle = np.sin(angle * np.pi / 180)
    cos_angle = np.cos(angle * np.pi / 180)
    vert = v * cos_angle + np.cross(u, v) * sin_angle + np.dot(u, v) * u * (1 - cos_angle)
    verts = vert.tolist()
    return verts

# 计算旋转后三维坐标系的各平面法向量
def normal_vector(x1,y1,z1,x2,y2,z2,x3,y3,z3):

    a = (y2-y1)*(z3-z1)-(y3-y1)*(z2-z1)
    b = (z2-z1)*(x3-x1)-(z3-z1)*(x2-x1)
    c = (x2-x1)*(y3-y1)-(x3-x1)*(y2-y1)
    verts = (a, b, c)
    return verts

# 计算旋转后三维坐标系的各平面与初始xoy平面夹角
def dihedral_angle(x, y, z):
    angle = math.acos((x * 0 + y * 0 + z * 1) / (x**2 + y**2 + z**2)**0.5) * 180 / math.pi
    return angle


def test(X, Y, Z):

    # 定义初始三维坐标系单位初始点坐标
    verts = [(0, 0, 0),(1, 0, 0), (0, 1, 0), (0, 0, 1)]

    # X轴，Y轴经旋转顺序后的初始向量坐标
    # 严格按照z-y-x旋转顺序进行计算
    Sz_Y = RodriguesRotate(Z, np.array(verts[2]), np.array(verts[3]))
    Sz_X = RodriguesRotate(Z, np.array(verts[1]), np.array(verts[3]))
    Szy_X = RodriguesRotate(Y, np.array(Sz_X), np.array(Sz_Y))
    print("y轴经过z轴旋转后状态：",Sz_Y)
    print("x轴经过z轴旋转后状态：",Sz_X)
    print("x轴经过z轴和y轴旋转后状态：",Szy_X)

    # 经旋转顺序后定义的单位点坐标
    # 严格按照z-y-x旋转顺序进行计算
    verts_z = [RodriguesRotate(Z, np.array(vert), np.array(verts[3])) for vert in verts]
    verts_zy = [RodriguesRotate(Y, np.array(vert), np.array(Sz_Y)) for vert in verts_z]
    verts_zyx = [RodriguesRotate(X, np.array(vert), np.array(Szy_X)) for vert in verts_zy]

    # 旋转后的三维坐标系的各个点组成的三个面
    xoy1 = [verts_zyx[0] + verts_zyx[1] + verts_zyx[2]]
    xoz1 = [verts_zyx[0] + verts_zyx[1] + verts_zyx[3]]
    yoz1 = [verts_zyx[0] + verts_zyx[2] + verts_zyx[3]]

    # 旋转后三维坐标系中三个平面的法向量
    normal_vector_xoy = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in xoy1]
    normal_vector_xoz = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in xoz1]
    normal_vector_yoz = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in yoz1]

    # 法向量与初始xoy平面法向量(oz向量)夹角
    # 两平面法向量夹角就是面面角
    dihedral_angle_xoy = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_xoy]
    dihedral_angle_xoz = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_xoz]
    dihedral_angle_yoz = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_yoz]
    print("旋转后三维坐标系中的xoy平面与初始xoy平面夹角：", dihedral_angle_xoy)
    print("旋转后三维坐标系中的xoz平面与初始xoy平面夹角：", dihedral_angle_xoz)
    print("旋转后三维坐标系中的yoz平面与初始xoy平面夹角：", dihedral_angle_yoz)

test(45,45,45) # 参数为欧拉角x,欧拉角y,欧拉角z

Python代码（外旋）（不使用numpy）

import math

# 计算绕某轴旋转后的点及顺序后的旋转轴
def RodriguesRotate(angle, x1, y1, z1, x2, y2, z2):

    sin_angle = math.sin(angle * math.pi / 180)
    cos_angle = math.cos(angle * math.pi / 180)

    x = x1 * cos_angle + (y2 * z1 - z2 * y1) * sin_angle + x2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    y = y1 * cos_angle + (z2 * x1 - x2 * z1) * sin_angle + y2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    z = z1 * cos_angle + (x2 * y1 - y2 * x1) * sin_angle + z2 * (x2 * x1 + y2 * y1 + z2 * z1) * (1 - cos_angle);
    verts = (x, y, z)
    return verts

# 计算旋转后三维坐标系的各平面法向量
def normal_vector(x1,y1,z1,x2,y2,z2,x3,y3,z3):

    a = (y2-y1)*(z3-z1)-(y3-y1)*(z2-z1)
    b = (z2-z1)*(x3-x1)-(z3-z1)*(x2-x1)
    c = (x2-x1)*(y3-y1)-(x3-x1)*(y2-y1)
    verts = (a, b, c)
    return verts

# 计算旋转后三维坐标系的各平面与初始xoy平面夹角
def dihedral_angle(x, y, z):
    angle = math.acos((x * 0 + y * 0 + z * 1) / (x**2 + y**2 + z**2)**0.5) * 180 / math.pi
    return angle


def test(X, Y, Z):

    # 定义初始三维坐标系单位初始点坐标
    verts = [(0, 0, 0),(1, 0, 0), (0, 1, 0), (0, 0, 1)]

    # 经旋转顺序后定义的单位点坐标
    # 严格按照z-y-x旋转顺序进行计算
    Sz_Y = RodriguesRotate(Z, verts[2][0], verts[2][1], verts[2][2], verts[3][0], verts[3][1], verts[3][2])
    Sz_X = RodriguesRotate(Z, verts[1][0], verts[1][1], verts[1][2], verts[3][0], verts[3][1], verts[3][2])
    Szy_X = RodriguesRotate(Y, Sz_X[0], Sz_X[1], Sz_X[2], Sz_Y[0], Sz_Y[1], Sz_Y[2])
    print("y轴经过z轴旋转后：", Sz_Y)
    print("x轴经过z轴旋转后：", Sz_X)
    print("x轴经过z轴和y轴旋转后：", Szy_X)

    # X轴，Y轴经旋转顺序后的初始向量坐标
    # 严格按照z-y-x旋转顺序进行计算
    verts_z = [RodriguesRotate(Z, vert[0], vert[1], vert[2], verts[3][0], verts[3][1], verts[3][2]) for vert in verts]
    verts_zy = [RodriguesRotate(Y, vert[0], vert[1], vert[2], Sz_Y[0], Sz_Y[1], Sz_Y[2]) for vert in verts_z]
    verts_zyx = [RodriguesRotate(X, vert[0], vert[1], vert[2], Szy_X[0], Szy_X[1], Szy_X[2]) for vert in verts_zy]

    # 旋转后的三维坐标系的各个点组成的三个面
    xoy1 = [verts_zyx[0] + verts_zyx[1] + verts_zyx[2]]
    xoz1 = [verts_zyx[0] + verts_zyx[1] + verts_zyx[3]]
    yoz1 = [verts_zyx[0] + verts_zyx[2] + verts_zyx[3]]

    # 旋转后三维坐标系中三个平面的法向量
    normal_vector_xoy = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in xoy1]
    normal_vector_xoz = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in xoz1]
    normal_vector_yoz = [normal_vector(vert[0], vert[1], vert[2], vert[3], vert[4], vert[5], vert[6], vert[7], vert[8])
                         for vert in yoz1]

    # 法向量与初始xoy平面法向量(oz向量)夹角
    # 两平面法向量夹角就是面面角
    dihedral_angle_xoy = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_xoy]
    dihedral_angle_xoz = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_xoz]
    dihedral_angle_yoz = [dihedral_angle(vert[0], vert[1], vert[2]) for vert in normal_vector_yoz]
    print("旋转后三维坐标系中的xoy平面与初始xoy平面夹角：", dihedral_angle_xoy)
    print("旋转后三维坐标系中的xoz平面与初始xoy平面夹角：", dihedral_angle_xoz)
    print("旋转后三维坐标系中的yoz平面与初始xoy平面夹角：", dihedral_angle_yoz)

test(45,45,45) # 参数为欧拉角x,欧拉角y,欧拉角z

运行结果

y轴经过z轴旋转后： (-0.7071067811865476, 0.7071067811865476, 0.0)
x轴经过z轴旋转后： (0.7071067811865476, 0.7071067811865476, 0.0)
x轴经过z轴和y轴旋转后： (0.5000000000000001, 0.5000000000000001, -0.7071067811865477)
旋转后三维坐标系中的xoy平面与初始xoy平面夹角： [60.00000000000001]
旋转后三维坐标系中的xoz平面与初始xoy平面夹角： [120.00000000000001]
旋转后三维坐标系中的yoz平面与初始xoy平面夹角： [135.0]

内旋算法参考：https://blog.youkuaiyun.com/weixin_49861340/article/details/131106922