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

目录硬知识Python代码使用方法串口收集数据椭球拟合验证STC15F2K60S2 16.384MHzKeil uVision V5.29.0.0PK51 Prof.Developers Kit Version:9.60.0.0Python 3.8.11 (default, Aug6 2021, 09:57:55) [MSC v.1916 64 bit (AMD64)] :: Anaconda,

乙酸氧铍

2107人浏览 · 2021-12-02 19:23:51

乙酸氧铍 · 2021-12-02 19:23:51 发布

硬知识

椭球面的标准方程为：
         $x-x_o)/A)^2+((y-y_o)/B)^2+((z-z_o)/C)^2=1$ ，
需要拟合的参数有
         $x_o，y_o，z_o，A，B，C$
六个，他们分别是椭球的球心坐标和半轴长。
将标准方程写成一般形式为：
         $x^2+ ay^2+ bz^2+cx+dy +ez+f=0$ ，
通过对参数 $a ， b ， c ， d ， e 、 f$ 的求解间接求出参数 $x_o，y_o，z_o，A，B，C$ 。
将实测得到的点代入一般形式，可得到对应的误差项，所有点的误差平方和记作
         $\sum_{i=1}^{N}e_i(a,b,c,d,e,f)^2$
求偏导数并令其为0：
         $\partial E/\partial a = 0$ ,
         $\partial E/\partial b = 0$ ,
         $\partial E/\partial c = 0$ ,
         $\partial E/\partial d = 0$ ,
         $\partial E/\partial e = 0$ ,
         $\partial E/\partial f = 0$
有
         $2y^4)*a+(2y^2z^2)*b+(2xy^2)*c+(2y^3)*d +(2y^2z)*e+(2y^2)*f+2x^2y^2=0$
         $2y^2z^2)*a +(2z^4)*b+(2xz^2)*c +(2yz^2)*d+(2z^3)*e +(2z^2)*f +2x^2z^2=0$
         $2xy^2)*a+(2xz^2)*b+(2x^2)*c+(2xy)*d+(2xz)*e +(2x)*f+2x^3=0$
         $2y^3)*a +(2yz^2)*b+(2xy)*c+(2y^2)*d+(2yz)*e+(2y)*f+2x^2y=0$
         $2y^2z)*a+(2z^3)*b+(2xz)*c+(2yz)*d+(2z^2)*e+(2z)*f+2x^2z=0$
         $2y^2)*a+(2z^2)*b+(2x)*c+(2y)*d+(2z)*e+(2)*f+2x^2=0$

解方程组可得 $a ， b ， c ， d ， e ， f$ ，进而可得 $x_o，y_o，z_o，A，B，C$
上面的六个等式中，设参数矩阵为 $A_{Matrix}$ ，常数项移至右边为 $B_{Matrix}$ ，参数项为 $x=[a,b,c,d,e,f]^T$
有 $A_{Matrix}·x=B_{Matrix}$
则 $x=A_{Matrix}^{-1}·B_{Matrix}$
         $x_o=-c/2$
         $y_o=-d/(2a)$
         $z_o=-e/(2b)$
         $\sqrt{x_o^2 + a · y_o^2 + b · z_o^2 - f}$
         $\sqrt{a}$
         $\sqrt{b}$

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:\windows\fonts\simsun.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 磁力计

串口收集数据

在这里插入图片描述

转动板子到各个角度
当觉得收集够时停止脚本

椭球拟合

开始椭球拟合
在这里插入图片描述
得到拟合结果：

验证

将计算结果用于矫正输出
在这里插入图片描述
清理掉旧数据后重新收集并拟合，得到如下结果，可见新的球心偏移较未矫正前小，且得到的椭球更接近正球。

在这里插入图片描述

天启AI社区

GitCode 天启AI是一款由 GitCode 团队打造的智能助手，基于先进的LLM（大语言模型）与多智能体 Agent 技术构建，致力于为用户提供高效、智能、多模态的创作与开发支持。它不仅支持自然语言对话，还具备处理文件、生成 PPT、撰写分析报告、开发 Web 应用等多项能力，真正做到“一句话，让 Al帮你完成复杂任务”。

更多推荐