CN104482944A - 一种基于椭圆拟合的地磁传感器动态校准方法 - Google Patents

Abstract

一种基于椭圆拟合的地磁传感器动态校准方法，将地磁传感器围绕固定点旋转一周，旋转过程中，间隔若干角度记录一次地磁坐标；将若干地磁坐标记录点作为椭圆拟合的输入点，做椭圆方程的变形，在地磁坐标记录点中，取出其中的n个点，n≥5，其坐标分别为：(x₁,y₁),(x₂,y₂),…,(x_n,y_n)，将坐标带入变形后的椭圆方程，构造方程组拟合参数，根据拟合参数对地磁进行校正。本发明用椭圆拟合方法，获得椭圆的中心点坐标、倾斜角度以及长、短半轴，对电磁传感器进行动态校准，解决了由于方位圆的圆心偏离原点产生偏移，使根据地磁传感器的测定值而计算出的方位与实际方位不同的问题。

Description

一种基于椭圆拟合的地磁传感器动态校准方法

技术领域

本发明涉及一种校准方法，尤其涉及一种基于椭圆拟合的地磁传感器动态校准方法。

背景技术

电磁传感器在固定磁场中以匀速缓慢旋转一圈或多圈时，根据其输出所绘制的圆被称作方位圆，地磁传感器具有沿着的水平面中的两个轴(X轴和Y轴方向)的地磁检测方向作为传感方向。由于地磁的分布特性呈现椭圆特点，方位圆的圆心偏离原点。该偏离被称作偏移，并且该偏离值被称作偏移值。当存在这种偏移时，根据没有偏移这样的假设，基于地磁传感器的测定值而计算出的方位与实际方位不同。

发明内容

本发明提供了一种基于椭圆拟合的地磁传感器动态校准方法，采用椭圆拟合方法，获得椭圆的中心点坐标、倾斜角度以及长、短半轴，对电磁传感器进行动态校准，解决了由于方位圆的圆心偏离原点产生偏移，使根据地磁传感器的测定值而计算出的方位与实际方位不同的问题。

本发明的技术方案为：一种基于椭圆拟合的地磁传感器动态校准方法，包括下述步骤：

S1.将地磁传感器围绕固定点旋转一周，旋转过程中，间隔若干角度记录一次地磁坐标；S2.将步骤S1中的若干地磁坐标记录点作为椭圆拟合的输入点，椭圆的一般方程如下：

Ax²+Bxy+Cy²+Dx+Ey+F＝0

对椭圆一般方程做如下变形：

-Cy²＝Ax²+Bxy+Dx+Ey+F   (1)

再对方程(1)作如下变形：

-y²＝A/Cx²+B/Cxy+D/Cx+E/Cy+F/C   (2)

A/C＝M₁；

B/C＝M₂；

令D/C＝M₃；

E/C＝M₄；

F/C＝M₅；

将M₁,M₂,M₃,M₄,M₅带入方程(2)得：

-y²＝M₁x²+M₂xy+M₃x+M₄y+M₅   (3)

对方程(3)进一步变形，用矩阵表示如下：

$-y^2=(x^2,\mathrm{xy},x,y,1)\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]---\left(4\right);$

S3.在地磁坐标记录点中，取出其中的n个点，n≥5，其坐标分别为：(x₁,y₁),(x₂,y₂),…,(x_n,y_n)，将坐标带入方程(4)，构造方程组拟合参数，则可得到如下n个方程构成的方程组：

$M_1{x_1}^2+M_2{x}_1{y}_1+M_3{x}_1+M_4{x}_1+M_5=-y_1^2---\left(1\right)$

$M_1{x_2}^2+M_2{x}_2{y}_2+M_3{x}_2+M_4{x}_2+M_5=-y_2^2---\left(2\right)$

……………………………………………

$M_1{x_n}^2+M_2{x}_n{y}_n+M_3{x}_n+M_4{x}_n+M_5=-y_n^2---\left(n\right)$

用矩阵Ax＝b表示为：

$\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right]\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ \·\\ \·\\ \·\\ -y_n^2\end{array}\right]$

则矩阵 $A=\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right],$ $x=\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right],$ $b=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ \·\\ \·\\ \·\\ -y_n^2\end{array}\right],$ 由最小二乘法求解，得到方程A^TAx＝A^Tb的精确解，即得到Ax＝b的最优解，其为参数M₁,M₂,M₃,M₄,M₅的值，进而得到椭圆方程；

S4根据椭圆方程，求出椭圆的倾斜角度、中心点坐标、长半轴的长度和短半轴的长度，具体步骤如下：

(1)令：

P1＝M1；

P2＝M2；

P3＝C＝1；

P4＝M3；

P5＝M4；

P6＝M5；

(2)椭圆的倾斜角度为：

$\mathrm{\θ}=\frac{1}{2}{\mathrm{tg}}^{-1}\frac{P_2}{P_1-P_3}$

令：

Ao＝P₆

Au＝P₄cosθ+P₅sinθ

Av＝-P₄sinθ+P₅cosθ

Auu＝P₁cos²θ+P₃sin²θ+P₂sinθcosθ

Avv＝P₁sin²θ+P₃cos²θ-P₂sinθcosθ

(3)椭圆中心点坐标为：

$\mathrm{xCenter}=\frac{-\mathrm{Au}\cos \mathrm{\θ}}{2\mathrm{Auu}}-\frac{-\mathrm{Av}\sin \mathrm{\θ}}{2\mathrm{Avv}}$

$\mathrm{yCenter}=\frac{-\mathrm{Au}\cos \mathrm{\θ}}{2\mathrm{Auu}}-\frac{-\mathrm{Av}\sin \mathrm{\θ}}{2\mathrm{Avv}}$

xCenter为椭圆在x方向的中心点坐标，yCenter为椭圆在y方向的中心点坐标；

(4)椭圆的长半轴和短半轴分别为：

$R_U=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Auu}}\right|}$

$R_V=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Avv}}\right|};$

R_U为椭圆长半轴的长度，R_V为椭圆短半轴的长度；

S6.地磁的校正：

(1)系数计算如下：

Cx＝Xsf*xCenter

Cy＝Ysf*yCenter

X_sf为地磁方位圆在x方向的偏移系数，Y_sf为地磁方位圆在y方向的偏移系数，C_x为地磁方位圆在x方向的圆心坐标，C_y为地磁方位圆在y方向的的圆心坐标。

(2)校正

H_x＝H_xbX_sf-C_x

H_y＝H_ybY_sf-C_y

其中，H_xb,H_yb是地磁的原始输出，H_x，H_y是校正后的输出。

有益效果：本发明利用地磁场的椭圆分布性质，使用椭圆拟合方法对地磁传感器进行动态校准减小了地磁传感器的计算方位与实际方位之间的偏差，提高了地磁传感器的测量精度。

具体实施方式

实施例：一种基于椭圆拟合的地磁传感器动态校准方法，包括下述步骤：

S1.将地磁传感器围绕固定点旋转一周，旋转过程中，间隔若干角度记录一次地磁坐标；S2.将步骤S1中的若干地磁坐标记录点作为椭圆拟合的输入点，椭圆的一般方程如下：

Ax²+Bxy+Cy²+Dx+Ey+F＝0

对椭圆一般方程做如下变形：

-Cy²＝Ax²+Bxy+Dx+Ey+F   (1)

再对方程(1)作如下变形：

-y²＝A/Cx²+B/Cxy+D/Cx+E/Cy+F/C   (2)

A/C＝M₁；

B/C＝M₂；

令D/C＝M₃；

E/C＝M₄；

F/C＝M₅；

将M₁,M₂,M₃,M₄,M₅带入方程(2)得：

-y²＝M₁x²+M₂xy+M₃x+M₄y+M₅   (3)

对方程(3)进一步变形，用矩阵表示如下：

$-y^2=(x^2,\mathrm{xy},x,y,1)\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]---\left(4\right);$

S3.在地磁坐标记录点中，取出其中的n个点，n≥5，其坐标分别为：(x₁,y₁),(x₂,y₂),…,(x_n,y_n)，将坐标带入方程(4)，构造方程组拟合参数，则可得到如下n个方程构成的方程组：

$M_1{x_1}^2+M_2{x}_1{y}_1+M_3{x}_1+M_4{x}_1+M_5=-y_1^2---\left(1\right)$

$M_1{x_2}^2+M_2{x}_2{y}_2+M_3{x}_2+M_4{x}_2+M_5=-y_2^2---\left(2\right)$

……………………………………………

$M_1{x_n}^2+M_2{x}_n{y}_n+M_3{x}_n+M_4{x}_n+M_5=-y_n^2---\left(n\right)$

用矩阵Ax＝b表示为：

$\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right]\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ \·\\ \·\\ \·\\ -y_n^2\end{array}\right]$

则矩阵 $A=\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right],$ $x=\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right],$ $b=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ \·\\ \·\\ \·\\ -y_n^2\end{array}\right],$ 由最小二乘法求解，得到方程A^TAx＝A^Tb的精确解，即得到Ax＝b的最优解，其为参数M₁,M₂,M₃,M₄,M₅的值，进而得到椭圆方程；

S4根据椭圆方程，求出椭圆的倾斜角度、中心点坐标、长半轴的长度和短半轴的长度，具体步骤如下：

(1)令：

P1＝M1；

P2＝M2；

P3＝C＝1；

P4＝M3；

P5＝M4；

P6＝M5；

(2)椭圆的倾斜角度为：

$\mathrm{\θ}=\frac{1}{2}{\mathrm{tg}}^{-1}\frac{P_2}{P_1-P_3}$

令：

Ao＝P₆

Au＝P₄cosθ+P₅sinθ

Av＝-P₄sinθ+P₅cosθ

Auu＝P₁cos²θ+P₃sin²θ+P₂sinθcosθ

Avv＝P₁sin²θ+P₃cos²θ-P₂sinθcosθ

(3)椭圆中心点坐标为：

$\mathrm{xCenter}=\frac{-\mathrm{Au}\cos \mathrm{\θ}}{2\mathrm{Auu}}-\frac{-\mathrm{Av}\sin \mathrm{\θ}}{2\mathrm{Avv}}$

$\mathrm{yCenter}=\frac{-\mathrm{Au}\cos \mathrm{\θ}}{2\mathrm{Auu}}-\frac{-\mathrm{Av}\sin \mathrm{\θ}}{2\mathrm{Avv}}$

xCenter为椭圆在x方向的中心点坐标，yCenter为椭圆在y方向的中心点坐标；

(4)椭圆的长半轴和短半轴分别为：

$R_U=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Auu}}\right|}$

$R_V=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Avv}}\right|};$

R_U为椭圆长半轴的长度，R_V为椭圆短半轴的长度；

S6.地磁的校正：

(1)系数计算如下：

Cx＝Xsf*xCenter

Cy＝Ysf*yCenter

X_sf为地磁方位圆在x方向的偏移系数，Y_sf为地磁方位圆在y方向的偏移系数，C_x为地磁方位圆在x方向的圆心坐标，C_y为地磁方位圆在y方向的的圆心坐标。

(2)校正

H_x＝H_xbX_sf-C_x

H_y＝H_ybY_sf-C_y

其中，H_xb,H_yb是地磁的原始输出，表示地磁传感器在x和y方向的磁感应强度，H_x，H_y是校正后的输出，表示地磁传感器校正后在x和y方向的磁感应强度。

具体的，表一为地磁传感器旋转一周的过程中在地磁坐标记录点中选取的36个坐标点；

表一

x	y
		-54764.21235	-1843.87654
-54143.92588	5415.928111
		-51113.79884	6724.957038
-51713.71237	9628.346205
		-49273.31239	11366.96874
-45734.95008	14020.06344
		-42694.63657	12274.11276
-39654.32306	14596.45035
		-31845.26079	15061.47815
-27584.74729	15928.39085
		-24605.55261	13333.24457
-20995.885	10874.6351
		-16755.74445	9133.746452
-16155.83092	5649.173249
		-13756.17683	1591.009051
-11336.14979	-1021.721873
		-10776.98216	-3910.057406
-10756.60921	-7114.096423
		-10166.88216	-12193.16232

-10197.44158	-14070.72021
		-13166.44978	-16855.28968
-17406.59033	-19182.68929
		-21616.17147	-20794.89908
-23446.47145	-23261.23405
		-25846.12555	-24433.72618
-31936.93903	-23117.50156
		-36746.43369	-23569.8743
-40325.54189	-21558.44307
		-43345.48245	-20691.79527
-47555.06359	-18816.90084
		-49375.17711	-14458.08679
-52974.65825	-11567.3527
		-54205.04471	-9381.684364
-54235.60413	-3122.665749
		-54164.29882	3389.310801
-52344.18531	6585.889226

将表一中的坐标带入方程(4)，构造方程组拟合参数，

(1)求出椭圆相关参数

P₁＝M₁＝0.758751；

P₂＝M₂＝0.203926；

P₃＝1；

P₄＝M₃＝50495.982236；

P₅＝M₄＝15264.400907；

P₆＝M₅＝478275032.615223；

(2)得到椭圆的一般方程为

0.758751x²+0.203926xy+y²+50495.982236x+15264.400907y+478275032.615223＝0(3)椭圆的倾斜角度：

$\mathrm{\θ}=\frac{1}{2}{\mathrm{tg}}^{-1}\frac{P_2}{P_1-P_3}=-20.103839$ 度；

(4)中心点坐标：

(5)椭圆的长半轴和短半轴：

$R_U=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Auu}}\right|}=22953.382214;$

$R_V=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Avv}}\right|}=19142.000744;$

(6)校正系数计算：

Xsf＝max[1,Rv/Ru]＝1；

Ysf＝max[1,Ru/Rv]＝1.19；

Cx＝Xsf*xCenter＝-32698.1072；

Cy＝Ysf*yCenter＝-5153.6931

(7)利用校正系数计算地磁的校正后的输出。

Claims (1)

1.一种基于椭圆拟合的地磁传感器动态校准方法，其特征在于，包括下述步骤：

S1.将地磁传感器围绕固定点旋转一周，旋转过程中，间隔若干角度记录一次地磁坐标；

S2.将步骤S1中的若干地磁坐标记录点作为椭圆拟合的输入点，椭圆的一般方程如下：

Ax²+Bxy+Cy²+Dx+Ey+F＝0

对椭圆一般方程做如下变形：

-Cy²＝Ax²+Bxy+Dx+Ey+F   (1)

再对方程(1)作如下变形：

-y²＝A/Cx²+B/Cxy+D/Cx+E/Cy+F/C   (2)

令 $\begin{array}{c}A/C=M_1;\\ B/C=M_2;\\ D/C=M_3;\\ E/C=M_4;\\ F/C=M_5;\end{array}$

将M₁,M₂,M₃,M₄,M₅带入方程(2)得：

-y²＝M₁x²+M₂xy+M₃x+M₄y+M₅   (3)

对方程(3)进一步变形，用矩阵表示如下：

$-y^2=(x^2,\mathrm{xy},x,y,1)\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]---\left(4\right);$

S3.在地磁坐标记录点中，取出其中的n个点，n≥5，其坐标分别为：(x₁,y₁),(x₂,y₂),…,(x_n,y_n)，将坐标带入方程(4)，构造方程组拟合参数，则可得到如下n个方程构成的方程组：

$M_1{x_1}^2+M_2{x}_1{y}_1+M_3{x}_1+M_4{x}_1+M_5=-y_1^2---\left(1\right)$

$M_1{x_2}^2+M_2{x}_2{y}_2+M_3{x}_2+M_4{x}_2+M_5=-y_2^2---\left(2\right)$

……………………………………………

$M_1{x_n}^2+M_2{x}_n{y}_n+M_3{x}_n+M_4{x}_n+M_5=-y_n^2---\left(n\right)$

用矩阵Ax＝b表示为：

$\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ .& .& .& .& .\\ .& .& .& .& .\\ .& .& .& .& .\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right]\×\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right]=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ .\\ .\\ .\\ -y_n^2\end{array}\right]$

则矩阵 $A=\left[\begin{array}{ccccc}{x}_1^2& x_1{y}_1& x_1& y_1& 1\\ x_2^2& x_2{y}_2& x_2& y_2& 1\\ .& .& .& .& .\\ .& .& .& .& .\\ .& .& .& .& .\\ x_n^2& x_n{y}_n& x_n& y_n& 1\end{array}\right],x=\left[\begin{array}{c}{M}_1\\ M_2\\ M_3\\ M_4\\ M_5\end{array}\right],b=\left[\begin{array}{c}-y_1^2\\ -y_2^2\\ .\\ .\\ .\\ -y_n^2\end{array}\right],$ 由最小二乘法求解，得到方程A^TAx＝A^Tb的精确解，即得到Ax＝b的最优解，其为参数M₁,M₂,M₃,M₄,M₅的值，进而得到椭圆方程；

S4根据椭圆方程，求出椭圆的倾斜角度、中心点坐标、长半轴的长度和短半轴的长度，具体步骤如下：

(1)令：

P1＝M1；

P2＝M2；

P3＝C＝1；

P4＝M3；

P5＝M4；

P6＝M5；

(2)椭圆的倾斜角度为：

$\mathrm{\θ}=\frac{1}{2}{\mathrm{tg}}^{-1}\frac{P_2}{P_1-P_3}$

令：

Ao＝P₆

Au＝P₄cosθ+P₅sinθ

Av＝-P₄sinθ+P₅cosθ

Auu＝P₁cos²θ+P₃sin²θ+P₂sinθcosθ

Avv＝P₁sin²θ+P₃cos²θ-P₂sinθcosθ

(3)椭圆中心点坐标为：

$\mathrm{xCenter}=\frac{-\mathrm{Au}\cos \mathrm{\θ}}{2\mathrm{Auu}}-\frac{-\mathrm{Av}\sin \mathrm{\θ}}{2\mathrm{Avv}}$

$\mathrm{yCenter}=\frac{-\mathrm{Au}\sin \mathrm{\θ}}{2\mathrm{Auu}}+\frac{-\mathrm{Av}\cos \mathrm{\θ}}{2\mathrm{Avv}}$

xCenter为椭圆在x方向的中心点坐标，yCenter为椭圆在y方向的中心点坐标；

(4)椭圆的长半轴和短半轴分别为：

$R_U=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Auu}}\right|}$

$R_V=\sqrt{\left|\frac{-(\mathrm{Ao}-\mathrm{Auu}{(\mathrm{Au}/2\mathrm{Auu})}^2-\mathrm{Avv}{(\mathrm{Av}/2\mathrm{Avv})}^2}{\mathrm{Avv}}\right|};$

R_U为椭圆长半轴的长度，R_V为椭圆短半轴的长度；

S6.地磁的校正：

(1)系数计算如下：

Xsf＝max[1,Rv/Ru]

Ysf＝max[1,Ru/Rv]

Cx＝Xsf*xCenter

Cy＝Ysf*yCenter

X_sf为地磁方位圆在x方向的偏移系数，Y_sf为地磁方位圆在y方向的偏移系数，C_x为地磁方位圆在x方向的圆心坐标，C_y为地磁方位圆在y方向的的圆心坐标；

(2)校正

H_x＝H_xbX_sf-C_x

H_y＝H_ybY_sf-C_y

其中，H_xb,H_yb是地磁的原始输出，H_x，H_y是校正后的输出。