上期我们介绍了如何使用STM32F103C8T6来获取HMC5883L数字磁力计的XYZ轴的磁场数据。

并且我们利用磁场数据来计算角度值以及Z轴倾角。

  但是这种调试的方法来查看数据确实太捞了，于是我们在这个的基础上，利用Python简单的对数据进行一下三维显示。

  在实际的使用过程中发现

主要是通过串口获取数据，之后用两个3D开源库pygame和OpenGL来显示数据。

    
def parse_serial_data(self, data):
        try:
            # 解析数据字符串
            values = data.strip().split(',')
            if len(values)== 3:
                h_value = float(values[0].split(':')[1])
                m_value = float(values[1].split(':')[1])
                i_value = float(values[2].split(':')[1])
                self.rotation_x = i_value
                self.rotation_y = h_value
                return True
        except:
            return False

  例如这里利用串口获取数据之后，解析获取角度还有倾角。之后绘图进行简单显示，这里就不作过多赘述了。

在使用传感器的过程中我发现：当我的Y轴方向和北极方向重叠（角度为0的时候）Z轴的倾角为30度左右，这个在上期测试的时候就发现了。

在Y轴方向和磁场南极重合的时候，Z轴倾角为负30度。Y轴方向和在东西方向时则没有偏差，Z轴倾角都是0。

杭州的纬度在30°左右，所以磁场方向和水平角度相差为30°左右，当我把模块方向转过来的时候，就会就正好相反。

当我们的防止方向在东西方向，由于东西方向不存在磁场，磁场与Y轴正交。因此Z轴的磁场强度分量为0。因此当模块旋转九十度之后，倾角会不变。因此模块在水平摆放的时候，对水平角度旋转的测量极为精确，但是改变倾角，其他角度旋转的时候，就没这么简单了。

这源自于我的计算方法的错误，模块的Y轴指向磁场方向的时候，这时候XYZ坐标和地理意义上的南北极坐标方向一致。（Z轴偏差先不考虑）

但是当旋转了模块之后，其实XYZ的坐标系也发生了变换，并不能简单的用之前的方法来计算XYZ轴。

  如果我们记原本的向量坐标为a1=（x1,y1,z1）那么之后我们得到的向量的结果为a2= (x2,y2,z2)。

  a2是在a1的基础上通过旋转获得的，因此我们可以找一个旋转矩阵P。

  旋转矩阵的表达式可以使用罗德里格公式

在此之前我们需要知道a2和a1的旋转轴，但是由于我们是任意旋转的，不知道旋转轴。因此我们要先假设一个旋转轴v，可以用a1叉乘a2归一化来获得。当然原理比较麻烦，直接上程序：

	void calculateRotationMatrix(HMC5883L_Measure_t rawData, HMC5883L_Measure_t measures,float R[3][3]) {
    // 1. 提取原始向量和旋转后的向量
    float raw[3] = { rawData.gauss.x, rawData.gauss.y, rawData.gauss.z };    
    float rotated[3] = { measures.gauss.x, measures.gauss.y, measures.gauss.z };
    // 2. 计算旋转轴 (叉积)
    float axis[3];    axis[0] = raw[1] * rotated[2] - raw[2] * rotated[1];  // y1*z2 - z1*y2
    axis[1] = raw[2] * rotated[0] - raw[0] * rotated[2];  // z1*x2 - x1*z2
    axis[2] = raw[0] * rotated[1] - raw[1] * rotated[0];  // x1*y2 - y1*x2
    // 计算旋转轴的长度 (叉积的模)
    float axisLength = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
    // 如果旋转轴长度为0，说明两个向量是平行的，不需要旋转
    if (axisLength == 0.0f) {
        // 向量平行时，旋转矩阵为单位矩阵
        R[0][0] = R[1][1] = R[2][2] = 1.0f;
        R[0][1] = R[0][2] = R[1][0] = R[1][2] = R[2][0] = R[2][1] = 0.0f;
        return;
    }
    // 将旋转轴单位化
    float kx = axis[0] / axisLength;
    float ky = axis[1] / axisLength;
    float kz = axis[2] / axisLength;
    // 3. 计算旋转角度 (通过点积)
    float dotProduct = raw[0]*rotated[0] + raw[1]*rotated[1] + raw[2]*rotated[2];
    float magnitudeRaw = sqrt(raw[0]*raw[0] + raw[1]*raw[1] + raw[2]*raw[2]);
    float magnitudeRotated = sqrt(rotated[0]*rotated[0] + rotated[1]*rotated[1] + rotated[2]*rotated[2]);
    float cosTheta = dotProduct / (magnitudeRaw * magnitudeRotated);
    float theta = acos(cosTheta);  // 旋转角度
    // 4. 使用罗德里格公式计算旋转矩阵
    float c = cos(theta);    float s = sin(theta);
    // 旋转矩阵 R
    R[0][0] = c + kx*kx*(1 - c);    R[0][1] = kx*ky*(1 - c) - kz*s;    R[0][2] = kx*kz*(1 - c) + ky*s;
    R[1][0] = ky*kx*(1 - c) + kz*s;
    R[1][1] = c + ky*ky*(1 - c);
    R[1][2] = ky*kz*(1 - c) - kx*s;
    R[2][0] = kz*kx*(1 - c) - ky*s;
    R[2][1] = kz*ky*(1 - c) + kx*s;
    R[2][2] = c + kz*kz*(1 - c);
}

  就可以得到a2 = R * a1；

  我们可以通过a2和a1逆推旋转矩阵R。

  之后再利用旋转矩阵，分别计算出XYZ三轴的旋转分量。


float radToDeg(float radians)
 {
    return radians * 180.0f / 3.1415;
}

void extractEulerAngles(float R[3][3], float* thetaX, float* thetaY, float* thetaZ) 
{    // 计算绕 Y 轴的旋转角度 (pitch)
    *thetaY = asin(-R[2][0]);    // 计算绕 X 轴的旋转角度 (roll)
    *thetaX = atan2(R[2][1], R[2][2]);    
// 计算绕 Z 轴的旋转角度 (yaw)
    *thetaZ = atan2(R[1][0], R[0][0]);    
// 将弧度转换为度数
    *thetaX = radToDeg(*thetaX);
    *thetaY = radToDeg(*thetaY);
    *thetaZ = radToDeg(*thetaZ);
}

  这样子就获得了在原本坐标系下的旋转角度。我们可以用这个值来修正模块姿态变换导致的测量变换。


嘉立创PCB

还没有评论，抢个沙发！