當前位置：首頁 > 编程语言 > python >内容正文

python

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

發布時間：2023/12/9 python 37 豆豆

生活随笔收集整理的這篇文章主要介紹了【51单片机快速入门指南】4.4.1：python串口接收磁力计数据并进行最小二乘法椭球拟合小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

硬知識

橢球面的標準方程為：
???????? $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：
???????? $?E/?a=0\partial E/\partial a = 0$ ,
???????? $?E/?b=0\partial E/\partial b = 0$ ,
???????? $?E/?c=0\partial E/\partial c = 0$ ,
???????? $?E/?d=0\partial E/\partial d = 0$ ,
???????? $?E/?e=0\partial E/\partial e = 0$ ,
???????? $?E/?f=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 serialser = serial.Serial(port='COM8',baudrate=1200,parity=serial.PARITY_NONE, # 校驗位stopbits=serial.STOPBITS_ONE, # 停止位bytesize=serial.EIGHTBITS # 數據位)First_Flag = 0while 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 FontPropertiesfrom numpy.linalg import invfrom numpy import arange, zerosfrom math import sqrt, sin, cosfrom matplotlib import pyplot as pltdef 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_pointsy_avr = sum(y) / num_pointsz_avr = sum(z) / num_points# 二次項均值xx_avr = sum(dot_Mul(x, x)) / num_pointsyy_avr = sum(dot_Mul(y, y)) / num_pointszz_avr = sum(dot_Mul(z, z)) / num_pointsxy_avr = sum(dot_Mul(x, y)) / num_pointsxz_avr = sum(dot_Mul(x, z)) / num_pointsyz_avr = sum(dot_Mul(y, z)) / num_points# 三次項均值xxx_avr = sum(dot_Mul(dot_Mul(x, x), x)) / num_pointsxxy_avr = sum(dot_Mul(dot_Mul(x, x), y)) / num_pointsxxz_avr = sum(dot_Mul(dot_Mul(x, x), z)) / num_pointsxyy_avr = sum(dot_Mul(dot_Mul(x, y), y)) / num_pointsxzz_avr = sum(dot_Mul(dot_Mul(x, z), z)) / num_pointsyyy_avr = sum(dot_Mul(dot_Mul(y, y), y)) / num_pointsyyz_avr = sum(dot_Mul(dot_Mul(y, y), z)) / num_pointsyzz_avr = sum(dot_Mul(dot_Mul(y, z), z)) / num_pointszzz_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_pointszzzz_avr = sum(dot_Mul(dot_Mul(dot_Mul(z, z), z), z)) / num_pointsxxyy_avr = sum(dot_Mul(dot_Mul(dot_Mul(x, x), y), y)) / num_pointsxxzz_avr = sum(dot_Mul(dot_Mul(dot_Mul(x, x), z), z)) / num_pointsyyzz_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_Matrixxo = -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) / 3kA = ABC_avr / AkB = ABC_avr / BkC = ABC_avr / Cxo = 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 = 90num_sita = 45alfa = arange(0, num_alpha) * 1 * PI / num_alphasita = arange(0, num_sita) * 2 * PI / num_sitaX = 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 磁力計

串口收集數據

轉動板子到各個角度
當覺得收集夠時停止腳本

橢球擬合

開始橢球擬合

得到擬合結果：

驗證

將計算結果用于矯正輸出

清理掉舊數據后重新收集并擬合，得到如下結果，可見新的球心偏移較未矯正前小，且得到的橢球更接近正球。

總結

以上是生活随笔為你收集整理的【51单片机快速入门指南】4.4.1：python串口接收磁力计数据并进行最小二乘法椭球拟合的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

上一篇： Ubuntu环境下Android反编译a
下一篇： vim配置php语法高亮