MPU9250磁力计航向计算偏差问题求助

传感器与校准说明 #

使用Adafruit的ICM20948（对应MPU9250）传感器，输出磁力计原始数据。采用以下方式校准：

• 以3D"8"字形移动传感器采集数据，待偏移量稳定后停止
• 偏移量计算方式：

x offset = (max_x+min_x)/2
y offset = (max_y+min_y)/2
z offset = (max_z+min_z)/2

• 原始数据减去对应偏移量，得到校准后数据

航向计算方法 #

参考磁力计航向计算文档，使用以下公式：

Direction (y>0) = 90 - [arcTAN(x/y)]*180/PI
Direction (y<0) = 270 - [arcTAN(x/y)]*180/PI
Direction (y=0, x<0) = 180.0
Direction (y=0, x>0) = 0.0

测试偏差数据 #

实际测试中，航向计算结果存在明显偏差，具体数据如下：

• 朝向正北（预期0°）时，校准后数据：

x  15.749999999999998
y  -0.9749999999999996
z  47.4

计算结果：40.44°

• 朝向正东（预期90°）时，校准后数据：

x  4.049999999999999
y  -14.475
z  46.949999999999996

计算结果：84.906°

• 朝向正西（预期270°）时，校准后数据：

x  1.3499999999999996
y  7.125
z  45.0

计算结果：75.774°

• 朝向正南（预期180°）时，校准后数据：

x  -11.55
y  -2.9250000000000007
z  46.5

计算结果：128.99°

代码实现 #

校准数据采集代码 #

def calibrationDataCollection():
    i2c = board.I2C()  # uses board.SCL and board.SDA
    icm = adafruit_icm20x.ICM20948(i2c)
    
    lx = kbhit.lxTerm()
    lx.start()
    x=[]
    y=[]
    z=[]
    while True:
        if lx.kbhit(): 
            c = lx.getch()
            c_ord = ord(c)
            if c_ord == 32: # Spacebar
                print("\nStop")
                break
        x.append(icm.magnetic[0])
        y.append(icm.magnetic[1])
        z.append(icm.magnetic[2])

        print((max(x)+min(x))/2)
        print((max(y)+min(y))/2)
        print((max(z)+min(z))/2)
        print()
        time.sleep(0.1)
    
    comp_df = pd.DataFrame({"x": x, "y": y, "z": z})
    comp_df["offset_x"] = (comp_df.x.max()+comp_df.x.min())/2
    comp_df["offset_y"] = (comp_df.y.max()+comp_df.y.min())/2
    comp_df["offset_z"] = (comp_df.z.max()+comp_df.z.min())/2
    toCSV(analysisPath, "Calibration_2.csv", comp_df)

校准后数据获取与航向计算代码 #

def Compass_2():
    i2c = board.I2C()  # uses board.SCL and board.SDA
    icm = adafruit_icm20x.ICM20948(i2c)

    while True:
        x = icm.magnetic[0]- 12.15 #(these are my calibration offsets)
        y = -icm.magnetic[1]+ 6.225
        z = icm.magnetic[2]+ 14.4
        print("x ", x)
        print("y ", y)
        print("z ", z)

        heading = magToHeading2([x,y,z])
        
        print("heading: ", heading)
        time.sleep(0.5)

航向计算函数 #

def magToHeading2(magnetic):
    heading = -1

    if magnetic[1] > 0:
        heading = 90-math.atan(magnetic[0]/magnetic[1]) * 180 / math.pi
    elif magnetic[1] < 0:
        heading = 270-math.atan(magnetic[0]/magnetic[1]) * 180 / math.pi
    elif magnetic[1] == 0:
        if magnetic[0] < 0:
            heading = 180.0
        else:
            heading = 0.0
    
    return round(heading,3)

疑问 #

当前计算出的航向始终存在偏差，请问是航向计算方法有误，还是校准步骤需要补充？

内容的提问来源于stack exchange，提问作者Park Bo