前言
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进行演示方便理解（感谢这位大神）
MPU6050缺陷
MPU6050在没有外部磁力传感器的作用下时，芯片就没法获取地磁数据（地球磁力），所以就会导致欧拉角z在不断改变（上电初始化后，在使用一段时间后回到初始状态欧拉角z会有莫名其妙的角度，并且内旋外旋都存在这个问题），欧拉角z不断改变的结果就是：在三维旋转坐标中，x轴和y轴的初始方向的改变。
算法衍生
虽然x轴和y轴的初始方向在不断改变，但他们所组成的xoy平面是不会改变的，所以在规定了一定旋转顺序旋转后的三维坐标各平面与初始xoy平面所构成的面面角就不会因为z轴（欧拉角z）的不确定性而发生改变，算法的构思适用于内旋和外旋，但是算法不同。
面面角取值范围为0°—180°，可根据欧拉角的正负号判定面面角的正负号，就能给出旋转角度为-180°—180°，从而就能得出所有旋转状态返回的三个面面角组成的数组唯一性。（这里只给出了C/C++的正负号判断，Python重写很简单，但是我懒😂）
C/C++代码（内旋）
// 绕X轴旋转坐标计算
double *rotate_X(double angle1, double x, double y, double z){
double angle = angle1 / 180.0 * M_PI; // 以弧度作为参数
double new_x = x;
double new_y = y * cos(angle) + z * sin(angle);
double new_z = -(y * sin(angle)) + z * cos(angle);
double *vert = new double [3];
vert[0] = new_x;
vert[1] = new_y;
vert[2] = new_z;
return vert;
}
// 绕Y轴旋转坐标计算
double *rotate_Y(double angle1, double x, double y, double z){
double angle = angle1 / 180.0 * M_PI; // 以弧度作为参数
double new_x = x * cos(angle) - z * sin(angle);
double new_y = y;
double new_z = x * sin(angle) + z * cos(angle);
double *vert = new double [3];
vert[0] = new_x;
vert[1] = new_y;
vert[2] = new_z;
return vert;
}
// 绕Z轴旋转坐标计算
double *rotate_Z(double angle1, double x, double y, double z){
double angle = angle1 / 180.0 * M_PI; // 以弧度作为参数
double new_x = x * cos(angle) + y * sin(angle);
double new_y = -(x * sin(angle)) + y * cos(angle);
double new_z = z;
double *vert = new double [3];
vert[0] = new_x;
vert[1] = new_y;
vert[2] = new_z;
return vert;
}
// 过原点平面法向量计算
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;
}
主函数：
// 定义初始点坐标
double verts[4][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
// 计算旋转后点坐标
double new_o[3] = {verts[0][0], verts[0][1], verts[0][2]};
double *x_rz = rotate_Z(z, verts[1][0], verts[1][1], verts[1][2]);
double *x_ry = rotate_Y(y, x_rz[0], x_rz[1], x_rz[2]);
double *new_x = rotate_X(x, x_ry[0], x_ry[1], x_ry[2]);
if(!x_rz) delete(x_rz);
if(!x_ry) delete(x_ry);
double *y_rz = rotate_Z(z, verts[2][0], verts[2][1], verts[2][2]);
double *y_ry = rotate_Y(y, y_rz[0],y_rz[1],y_rz[2]);
double *new_y = rotate_X(x, y_ry[0],y_ry[1],y_ry[2]);
if(!y_rz) delete(y_rz);
if(!y_ry) delete(y_ry);
double *z_rz = rotate_Z(z, verts[3][0], verts[3][1], verts[3][2]);
double *z_ry = rotate_Y(y, z_rz[0],z_rz[1],z_rz[2]);
double *new_z = rotate_X(x, z_ry[0],z_ry[1],z_ry[2]);
if(!z_rz) delete(z_rz);
if(!z_ry) delete(z_ry);
// 旋转后的三维坐标系的点组成的三个面的法向量
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代码（内旋）
import math
# 绕X轴旋转坐标计算
def rotate_X(angle, x, y, z):
angle = math.radians(angle) # 以弧度作为参数
new_x = x
new_y = y * math.cos(angle) + z * math.sin(angle)
new_z = -(y * math.sin(angle)) + z * math.cos(angle)
verts = (new_x, new_y, new_z)
return verts
# 绕Y轴旋转坐标计算
def rotate_Y(angle, x, y, z):
angle = math.radians(angle) # 以弧度作为参数
new_x = x * math.cos(angle) - z * math.sin(angle)
new_y = y
new_z = x * math.sin(angle) + z * math.cos(angle)
verts = (new_x, new_y, new_z)
return verts
# 绕Z轴旋转坐标计算
def rotate_Z(angle, x, y, z):
angle = math.radians(angle) # 以弧度作为参数
new_x = x * math.cos(angle) + y * math.sin(angle)
new_y = -(x * math.sin(angle)) + y * math.cos(angle)
new_z = z
verts = (new_x, new_y, new_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)]
# 计算旋转后点坐标
verts_z = [rotate_Z(Z, vert[0], vert[1], vert[2]) for vert in verts]
verts_zy = [rotate_Y(Y, vert[0], vert[1], vert[2]) for vert in verts_z]
verts_zyx = [rotate_X(X, vert[0], vert[1], vert[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平面法向量夹角
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)运行结果
旋转后三维坐标系中的xoy平面与初始xoy平面夹角： [60.00000000000001]
旋转后三维坐标系中的xoz平面与初始xoy平面夹角： [81.57894188185058]
旋转后三维坐标系中的yoz平面与初始xoy平面夹角： [31.399714809919047]外旋算法参考：https://blog.youkuaiyun.com/weixin_49861340/article/details/131093279
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
