[发明专利]一种基于椭圆拟合的地磁传感器动态校准方法

摘要

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

主权项

一种基于椭圆拟合的地磁传感器动态校准方法，其特征在于，包括下述步骤：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*xCenterCy＝Ysf*yCenterX_sf为地磁方位圆在x方向的偏移系数，Y_sf为地磁方位圆在y方向的偏移系数，C_x为地磁方位圆在x方向的圆心坐标，C_y为地磁方位圆在y方向的的圆心坐标；(2)校正H_x＝H_xbX_sf‑C_xH_y＝H_ybY_sf‑C_y其中，H_xb,H_yb是地磁的原始输出，H_x，H_y是校正后的输出。