【51单片机快速入门指南】4.4.1：python串口接收磁力计数据并进行最小二乘法椭球拟合

STC15F2K60S2 16.384MHz

Keil uVision V5.29.0.0

PK51 Prof.Developers Kit Version:9.60.0.0

Python 3.8.11 (default, Aug 6 2021, 09:57:55) [MSC v.1916 64 bit (AMD64)] :: Anaconda, Inc. on win32

参考资料：

笔记：python读取串口数据并保到本地txt文件 —— 大头工程师笔记

最小二乘法拟合—基本原理 —— 铁头娃-wefly

硬知识

Python代码

if not 1:   # 0为串口收集数据，1为椭球拟合

    import serial

    ser = serial.Serial(

        port='COM8',

        baudrate=1200,

        parity=serial.PARITY_NONE,  # 校验位

        stopbits=serial.STOPBITS_ONE,  # 停止位

        bytesize=serial.EIGHTBITS  # 数据位

    )

    First_Flag = 0

    while True:

        data = ser.readline()

        if First_Flag:  # 丢掉第一次的，避免写入半截数据

            f = open('./Data.txt', 'a')

            data = data.decode('utf-8')[:-1]

            f.write(data)

            f.close()

            print(data)

        First_Flag = 1

else:

    Data_Path = r'./Data.txt'

    f = open(Data_Path, 'r')

    X = []

    Y = []

    Z = []

    for _ in f:

        List = _.replace(",", " ").split()

        X.append(int(List[0]))

        Y.append(int(List[1]))

        Z.append(int(List[2]))

    f.close()

    from matplotlib.font_manager import FontProperties

    from numpy.linalg import inv

    from numpy import arange, zeros

    from math import sqrt, sin, cos

    from matplotlib import pyplot as plt

    def dot_Mul(arr1, arr2):

        return [a * b for a, b in zip(arr1, arr2)]

    PI = 3.1415926535897932384626433832795

    # 实测数据

    f = open(Data_Path, 'r')

    x = []

    y = []

    z = []

    for _ in f:

        List = _.replace(",", " ").split()

        x.append(int(List[0]))

        y.append(int(List[1]))

        z.append(int(List[2]))

    f.close()

    # 数据总数

    num_points = len(x)

    # 一次项均值

    x_avr = sum(x) / num_points

    y_avr = sum(y) / num_points

    z_avr = sum(z) / num_points

    # 二次项均值

    xx_avr = sum(dot_Mul(x, x)) / num_points

    yy_avr = sum(dot_Mul(y, y)) / num_points

    zz_avr = sum(dot_Mul(z, z)) / num_points

    xy_avr = sum(dot_Mul(x, y)) / num_points

    xz_avr = sum(dot_Mul(x, z)) / num_points

    yz_avr = sum(dot_Mul(y, z)) / num_points

    # 三次项均值

    xxx_avr = sum(dot_Mul(dot_Mul(x, x), x)) / num_points

    xxy_avr = sum(dot_Mul(dot_Mul(x, x), y)) / num_points

    xxz_avr = sum(dot_Mul(dot_Mul(x, x), z)) / num_points

    xyy_avr = sum(dot_Mul(dot_Mul(x, y), y)) / num_points

    xzz_avr = sum(dot_Mul(dot_Mul(x, z), z)) / num_points

    yyy_avr = sum(dot_Mul(dot_Mul(y, y), y)) / num_points

    yyz_avr = sum(dot_Mul(dot_Mul(y, y), z)) / num_points

    yzz_avr = sum(dot_Mul(dot_Mul(y, z), z)) / num_points

    zzz_avr = sum(dot_Mul(dot_Mul(z, z), z)) / num_points

    # 四次项均值

    yyyy_avr = sum(dot_Mul(dot_Mul(dot_Mul(y, y), y), y)) / num_points

    zzzz_avr = sum(dot_Mul(dot_Mul(dot_Mul(z, z), z), z)) / num_points

    xxyy_avr = sum(dot_Mul(dot_Mul(dot_Mul(x, x), y), y)) / num_points

    xxzz_avr = sum(dot_Mul(dot_Mul(dot_Mul(x, x), z), z)) / num_points

    yyzz_avr = sum(dot_Mul(dot_Mul(dot_Mul(y, y), z), z)) / num_points

    # 系数矩阵

    A_Matrix = [[yyyy_avr, yyzz_avr, xyy_avr, yyy_avr, yyz_avr, yy_avr],

                [yyzz_avr, zzzz_avr, xzz_avr, yzz_avr, zzz_avr, zz_avr],

                [xyy_avr, xzz_avr, xx_avr, xy_avr, xz_avr, x_avr],

                [yyy_avr, yzz_avr, xy_avr, yy_avr, yz_avr, y_avr],

                [yyz_avr, zzz_avr, xz_avr, yz_avr, zz_avr, z_avr],

                [yy_avr, zz_avr, x_avr, y_avr, z_avr, 1]]

    # 等式右边的常数项矩阵

    B_Matrix = [[-xxyy_avr], [-xxzz_avr], [-xxx_avr], [-xxy_avr], [-xxz_avr], [-xx_avr]]

    result = inv(A_Matrix) @ B_Matrix

    xo = -result[2] / 2  # 拟合出的x坐标

    yo = -result[3] / (2 * result[0])  # 拟合出的y坐标

    zo = -result[4] / (2 * result[1])  # 拟合出的z坐标

    # 拟合出的x方向上的轴半径

    A = sqrt(xo * xo + result[0] * yo * yo + result[1] * zo * zo - result[5])

    # 拟合出的y方向上的轴半径

    B = A / sqrt(result[0])

    # 拟合出的z方向上的轴半径

    C = A / sqrt(result[1])

    ABC_avr = (A + B + C) / 3

    kA = ABC_avr / A

    kB = ABC_avr / B

    kC = ABC_avr / C

    xo = xo[0]

    yo = yo[0]

    zo = zo[0]

    print("拟合结果: ")

    print("xo = ", xo)  # 椭球球心x坐标

    print("yo = ", yo)  # 椭球球心y坐标

    print("zo = ", zo)  # 椭球球心z坐标

    print("A = ", A)  # 拟合出的x方向上的轴半径

    print("B = ", B)  # 拟合出的y方向上的轴半径

    print("C = ", C)  # 拟合出的z方向上的轴半径

    print("kA = ", kA)

    print("kB = ", kB)

    print("kC = ", kC)

    num_alpha = 90

    num_sita = 45

    alfa = arange(0, num_alpha) * 1 * PI / num_alpha

    sita = arange(0, num_sita) * 2 * PI / num_sita

    X = zeros((num_alpha, num_sita))

    Y = zeros((num_alpha, num_sita))

    Z = zeros((num_alpha, num_sita))

    for i in range(0, num_alpha):

        for j in range(0, num_sita):

            X[i, j] = xo + A * sin(alfa[i]) * cos(sita[j])

            Y[i, j] = yo + B * sin(alfa[i]) * sin(sita[j])

            Z[i, j] = zo + C * cos(alfa[i])

    X = [i for arr in X for i in arr]

    Y = [i for arr in Y for i in arr]

    Z = [i for arr in Z for i in arr]

    fig = plt.figure()

    Font = FontProperties(fname=r"c:windowsfontssimsun.ttc", size=20)

    ax1 = fig.add_subplot(221, projection='3d')

    ax1.set_title('实测、拟合对比', fontproperties=Font)

    ax1.scatter3D(X, Y, Z)  # 拟合

    ax1.scatter3D(x, y, z)  # 实测

    ax2 = fig.add_subplot(222)

    ax2.set_title('x-y投影', fontproperties=Font)

    ax2.scatter(X, Y)

    ax2.scatter(x, y)

    ax3 = fig.add_subplot(223)

    ax3.set_title('x-z投影', fontproperties=Font)

    ax3.scatter(X, Z)

    ax3.scatter(x, z)

    ax4 = fig.add_subplot(224)

    ax4.set_title('y-z投影', fontproperties=Font)

    ax4.scatter(Y, Z)

    ax4.scatter(y, z)

    plt.show()

使用方法

HMC5883L、QMC5883L的驱动程序见【51单片机快速入门指南】4.4：I2C 读取HMC5883L / QMC5883L 磁力计

串口收集数据

转动板子到各个角度

当觉得收集够时停止脚本

椭球拟合

开始椭球拟合

得到拟合结果：

验证

将计算结果用于矫正输出

清理掉旧数据后重新收集并拟合，得到如下结果，可见新的球心偏移较未矫正前小，且得到的椭球更接近正球。