CN103630129B - 动磁场下的滤波定位方法及装置 - Google Patents

Abstract

本发明属于磁场定位技术领域，公开了一种动磁场下的滤波定位方法及装置，本发明所提供的动磁场下的滤波定位方法及装置通过对采集的旋转磁场数据和姿态数据进行分段处理，将每段旋转磁场数据输入到旋转磁场模型进行解算获取每段旋转磁场数据的测量值，对上一段旋转磁场数据的滤波估计实施状态转移，得到上一段旋转磁场数据滤波的一步预测，将一步预测值和该段旋转磁场数据的测量值输入Kalman滤波器进行滤波，获取该段磁场数据的滤波估计，最终计算出目标物体的当前位置，大幅降低了定位结果的方差，具有更高的定位精度，实现在目标物体连续运动过程中的实时测量定位，提高了动磁场定位的抗干扰能力。

Description

动磁场下的滤波定位方法及装置

技术领域

本发明涉及磁场定位技术领域，特别是涉及一种动磁场下的滤波定位方法及装置。

背景技术

磁场定位技术是利用磁体随着其磁矩方向和距离的变化会在空间中形成的特定规律的磁场分布，通过检测特定磁场参数的变化测量该磁体在空间位置的变化，并确定观测点与磁体在空间中的相对位置。磁场定位算法在医学，导航，雷达，钻井等方面有着广泛的应用。在工业生产和检测中，一般采用专用的仪器设备来测量某些特殊的磁场强度分布情况，如舰船、鱼雷离线磁定位，磁靶向药物磁场定位，以及在航空航天中的一些应用。医学上将永磁体固连在微型吞入式医疗胶囊内由口腔服入，通过建立永磁体空间磁场与空间方位的简化定位模型，根据定位模型确定具体的定位方案，对微型医疗胶囊进入人体消化道后的二维或三维位置进行测量，提出了不可见状态下的非接触式永磁定位方法。水雷磁引信在工作周期内利用2~3个磁传感器，依靠读取一条完整或部分磁场通过曲线，对目标舰船进行位置、航速等运动状态辨识，进行实时定位，从而对舰船关键部位实施精确爆炸打击。上述磁定位方式中，所观测的磁场在短时间内都是静止或缓慢平移的，由观测的磁场大小和方位通过磁场的空间分布规律反推其所在位置。然而，实际作业中，所观测的静磁场混杂在众多噪声和干扰中，如地磁场和工频交流电产生的动磁场等，对于该静磁场的提取误差较大，影响最终定位精度。因而磁场定位在航空航天领域逐渐被无线电导航所取代，仅保留其作为一种应急导航定位手段。

20世纪80年代，国外出现了一种利用旋转的动磁场进行定位的技术，采用旋转磁场测定待测点与参考点之间相对距离、方位角和俯仰角。动磁场不仅具有空间分布的性质，同时具有一定的时频特性。动磁场定位具有精度高、抗干扰的特点，一些在时域中难以消除的干扰磁场可以变换至频域中处理。Arthur F.Kuckes等人已将该种旋转磁场定位方法形成专利US20030085059A1，并成功应用于将钻井导向中。然而，由于动磁场的时变特性，实际观测中需要采集较长一段信号方能对磁场信息作出准确的解析，造成了动磁场定位仅能应用于定点测量或在低速运动中近似将一段时间内的信号认为是测量末点的数据，严重影响了导航定位的实时性和连续性。同时，由于远场磁场衰减，测量结果在噪声影响下波动十分剧烈，甚至无法进行解算。

发明内容

（一）要解决的技术问题

本发明提供一种动磁场下的滤波定位方法，用以解决动磁场定位实时性、连续性差，受噪声干扰波动剧烈的问题。

（二）技术方案

为了解决上述技术问题，本发明提供一种动磁场下的滤波定位方法，包括：

100、在目标物体的运动过程中不断采集更新目标物体的旋转磁场数据和姿态数据并存储，同时根据预设规则生成一组时间节点，其中，旋转磁场数据包括x、y、z三个方向的磁场B_x、B_y、B_z及采样时间，姿态数据包括俯仰角方位角θ_s及采样时间；

101、k时刻，根据所述一组时间节点分别将存储的旋转磁场数据和姿态数据分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数；

102、将第m段旋转磁体磁场数据s_m输入旋转磁场模型进行解算，得到第m个测量值

103、当2≤m≤M时，根据第m段姿态数据h_m和第m个测量值对第m-1次Kalman滤波估计实施状态转移，得到第m-1次滤波估计的一步预测

即

或

其中，；

104、将第m次测量值和第m-1次滤波估计的一步预测输入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m=1时，则将k-1时刻第M次滤波估计作为k时刻第0次滤波估计的一步预测；

105、重复102、103和104步骤，并最终得到k时刻的第M次滤波估计则k时刻目标物体的绝对空间坐标为：

$\begin{array}{c}{z}^{\left(k\right)}={\mathrm{\γ}}_M^{\left(k\right)}\sin {\mathrm{\θ}}_M^{\left(k\right)}\\ .\end{array}$

同时，本发明还提供一种实现如上所述的动磁场下的滤波定位方法的装置，该装置包括安装在目标物体上的旋转磁体和姿态传感器、位置已知并用于检测所述旋转磁体旋转磁场的磁传感器、第一移位寄存器、第二移位寄存器、数据分段器、时间戳、旋转磁场模型和Kalman滤波器；

其中，所述第一移位寄存器和第二移位寄存器分别用于存储所述磁传感器感应的旋转磁场数据和所述姿态传感器感应的目标物体的姿态数据并不断更新；

所述时间戳用于根据预设规则生成一组时间节点；

所述数据分段器用于根据所述一组时间节点将所述第一移位寄存器中的旋转磁场数据和所述第二移位寄存器中的姿态数据分别分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数，并将分段后的旋转磁场数据s_m依次输入所述旋转磁场模型，分段后的姿态数据h_m依次输入Kalman滤波器；

所述旋转磁场模型依次对每段旋转磁场数据s_m进行解算，并将解算结果作为第m个测量值输入Kalman滤波器；

当2≤m≤M时，所述Kalman滤波器首先结合第m段姿态数据h_m和第m个测量值对第m-1次滤波估计实施状态转移，得到第m-1次滤波估计的一步预测然后将第m-1次滤波估计的一步预测及第m个测量值带入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m=1时，则将k-1时刻第M次滤波估计作为k时刻第0次滤波估计的一步预测；直至得到k时刻的第M次滤波估计

（三）有益效果

本发明所提供的动磁场下的滤波定位方法及装置通过对采集的旋转磁场数据和姿态数据进行分段处理，将每段旋转磁场数据输入到旋转磁场模型进行解算获取每段旋转磁场数据的测量值，并结合相应地姿态数据对上一段旋转磁场数据的滤波估计实施状态转移，得到上一段旋转磁场数据滤波的一步预测，将一步预测和该段旋转磁场数据的测量值输入Kalman滤波器进行滤波，获取该段磁场数据的滤波估计，并依次向外逐段推算，最终计算出目标物体的当前位置，大幅降低了定位结果的方差，具有更高的定位精度，并解决了远场定位能力不足、甚至无法定位的缺陷，实现在目标物体连续运动过程中的实时测量定位，提高了动磁场定位的抗干扰能力。

附图说明

图1为本发明实施例一中实现动磁场下的滤波定位方法装置的结构示意图；

图2为本发明实施例一中动磁场下的滤波定位方法的流程图；

图3为本发明实施例一中空间坐标系下目标物体运动过程的示意图；

图4为本发明实施例一中对旋转磁场数据和姿态数据的分段流程图；

图5为本发明实施例二中目标物体连续运动的场景示意图；

图6a为本发明实施例二中旋转磁场的分布示意图；

图6b为图6a中64.0~69.4s之间旋转磁场的局部放大分布示意图；

图7a为本发明实施例二中Kalman滤波后输出的目标物体的相对距离的测量值、滤波估计及一步预测的结果比较图；

图7b为本发明实施例二中Kalman滤波后输出的目标物体的俯仰角的测量值、滤波估计及一步预测的结果比较图；

图7c为本发明实施例二中Kalman滤波后输出的目标物体的方位角的测量值、滤波估计及一步预测的结果比较图；

图8a为本发明实施例二中目标物体整个运动过程中不加滤波和加Kalman滤波输出的目标物体的相对距离的结果比较图；

图8b为图8a中目标物体运动到50s附近时不加滤波和加Kalman滤波输出的目标物体的相对距离结果比较图的局部放大示意图；

图8c为图8a中目标物体运动到82.67s附近时不加滤波和加Kalman滤波输出的目标物体的相对距离结果比较图的局部放大示意图；

图9a为本发明实施例二中目标物体运动过程中在67.51~69.4s之间受强干扰时的旋转磁场分布示意图；

图9b为本发明实施例二中目标物体运动过程中在67.51~69.4s之间受强干扰时不加滤波和加Kalman滤波输出的目标物体的相对距离的测量值、滤波估计及一步预测的结果比较图；

图9c为本发明实施例二中目标物体运动过程中在67.51~69.4s之间受强干扰时不加滤波和加Kalman滤波输出的目标物体的方位角的测量值、滤波估计及一步预测的结果比较图；

其中，a：目标物体；1：旋转磁体；2：姿态传感器；3：磁传感器；4：第一移位寄存器；5：第二移位寄存器；6：数据分段器；7：时间戳；8：旋转磁场模型；9：Kalman滤波器；10：连接线。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本发明采用数字信号处理中的滤波方法对传统的旋转磁场定位算法进行改进。

图1所示为本发明实施例中实现动磁场下的滤波定位方法装置的结构示意图，如图1所示，该装置包括旋转磁体1、姿态传感器2、磁传感器3、第一移位寄存器4、第二移位寄存器5、数据分段器6、时间戳7、旋转磁场模型8和Kalman滤波器9，其中，旋转磁体1和姿态传感器2均安装于目标物体a上，在目标物体a连续运动过程中，旋转磁体1不断产生时变旋转磁场，同时姿态传感器2实时测量目标物体a的姿态数据。而磁传感器3放置于一已知位置，用于感应旋转磁体1的时变旋转磁场，通常距离目标物体a20~100m，作为相对导航定位的参考点。第一移位寄存器4和第二移位寄存器5分别用于存储磁传感器3感应的旋转磁场数据和姿态传感器2感应的目标物体a的姿态数据并不断更新，同时为每一个旋转磁场数据和姿态数据标记采样时间。

本实施例中时间戳7可以通过动力线或其他连接线10与目标物体a相连，并以连接线10长度变化计算被目标物体a实际运动距离（简称为“进尺”），同时根据进尺设定一组时间节点，如：连接线10长度每变化20cm，就设定一个时间节点；数据分段器6则根据时间戳7中的时间节点将第一移位寄存器4中的旋转磁场数据和第二移位寄存器5中的姿态数据分别分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数，并将分段后的旋转磁场数据s_m依次输入旋转磁场模型8，分段后的姿态数据h_m依次输入Kalman滤波器9。

旋转磁场模型8给出旋转磁场的空间分布规律，依次对每段旋转磁场数据s_m进行解算，并将解算结果作为第m个测量值输入Kalman滤波器；当2≤m≤M时，Kalman滤波器9首先结合第m段姿态数据h_m和第m个测量值对第m-1次Kalman滤波估计实施状态转移，得到第m-1次滤波的一步预测然后将第m-1次滤波估计的一步预测及第m个测量值带入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m=1时，则将k-1时刻第M次滤波估计视为第0次滤波估计的一步预测，直至得到k时刻的第M次滤波估计具体过程将在后面的内容中描述。

本发明所提供的实现动磁场下的滤波定位方法的装置通过对存储的旋转磁场数据和姿态数据进行分段，将分段后的旋转磁场数据依次输入旋转磁场模型进行解算，并将解算结果作为测量值输入Kalman滤波器，将分段后的姿态数据也输入Kalman滤波器，根据本发明更新的Kalman滤波方程对每段旋转磁场数据进行滤波，并最终计算得出目标物体当前位置信息，解决了现有动磁场定位实时性、连续性差，受噪声干扰波动剧烈的问题。

图2所示为本发明实施例中动磁场下的滤波定位方法的流程图。本发明实施例中动磁场下的滤波定位方法包括：

100、在目标物体的运动过程中不断采集更新目标物体的旋转磁场数据和姿态数据并存储，同时根据预设规则生成一组时间节点，其中，旋转磁场数据包括x、y、z三个方向的磁场B_x、B_y、B_z及采样时间，姿态数据包括俯仰角方位角θ_s及采样时间；

图3所示为空间坐标系下目标物体运动过程的示意图，结合图1和图3所示的空间坐标系下，目标物体a的实际运动轨迹为

设磁传感器3的采样率为f_s，第一移位寄存器4的存储容量为4×V，第一移位寄存器4中数据的更新周期等于磁传感器3的采样周期T_s=1/f_s，则任一时刻t第一移位寄存器4中的旋转磁场数据为：

s_V(n)=s(n+f_st),n=0,1,..,V-1

其中，s_V(n)为磁传感器3采集的旋转磁场数据，每一个旋转磁场数据包括x、y、z三个方向的磁场及采样时间，表示为(B_x;B_y;B_z;t)。

若目标物体a位置更新周期T_P，自时刻t=VT_s输出第一个位置，则目标物体a位置更新时间依次为：

t=VT_s+kT_p,k=1,2,...,∞，

则在时刻VT_s+kT_p（以下简称k时刻），第一移位寄存器4中的数据为：

$s_V^{\left(k\right)}\left(n\right)=s(n+V+{\mathrm{kf}}_s{T}_p),k=\mathrm{1,2},...,\∞$

同样的，设姿态传感器2的采样率为f_h，第二移位寄存器5的存储容量为4×U，每一个姿态数据包括被目标物体a瞬时俯仰角、方位角及采样时间，表示为k时刻，第二移位寄存器5中的姿态数据为:

$h_U^{\left(k\right)}\left(n\right)=h(n+U+{\mathrm{kf}}_h{T}_p),k=\mathrm{1,2},...,\∞$

由于磁场在空间的变化是连续的，可以认为在较小的范围Δl内的磁场相同，即将目标物体a移动较短一段距离内磁传感器3采集的信号作为目标物体a在同一点时产生的磁场，通常Δl不超过20cm。优选本实施例中当目标物体每移动Δl距离时，增加一个时间节点，形成时间戳7的时间节点集合t。

101、k时刻，根据生成的一组时间节点分别将存储的旋转磁场数据姿态数据分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数；

图4所示为本发明实施例中对旋转磁场数据和姿态数据的分段流程图。如图4所示，本实施例中优选对旋转磁场数据和姿态数据进行分段的过程具体为：

首先获取第一移位寄存器4中存储的磁场数据的第一个采样时间t_A，在时间戳7中找到与t_A最接近的时间节点t₀。然后依次在时间戳7中取t₀后面的时间节点t₁、t₂……t_M，并在第一移位寄存器4中的旋转磁场数据中寻找与时间节点t₁、t₂……t_M相同的采样时间，直至时间戳7的时间节点t_M+1寻找不到相同的采样时间，并以存储的旋转磁场数据的最后一个采样时间t_B替换t_M。根据磁场数据中已找到的M+1个采样时间t_A、t₁、t₂……t_B将旋转磁场数据分为M段，记t_A-t₁段磁场数据为s₁，t₁-t₂段磁场数据为s₂，……,t_M-1-t_B段磁场数据为s_M；

对磁场数据分段后，根据时间节点t_A、t₁、t₂……t_B将存储的姿态数据也分为M段，具体可以为：根据磁场数据的第一个t_A和最后一个采样时间t_B，在姿态数据中寻找与t₁、t₂……t_M-1相同的采样时间，每两个时间点之间的数据即为相应的分段姿态数据，记t_A-t₁段姿态数据为h₁，t₁-t₂段姿态数据为h₂，……，t_M-1-t_B段数据为h_M，分别对应于磁场数据s₁，s₂，……，s_M。

102、将第m段旋转磁体磁场数据s_m输入旋转磁场模型进行解算，得到第m个测量值

对旋转磁场数据和姿态数据完成分段后，依次将各段旋转磁场数据s_m输入旋转磁场模型8。旋转磁场模型8给出旋转磁体在空间中任意点(x,y,z)处产生的磁场

B(t)=B₁cos(ωt)+B₂sin(ωt)

$B_1=\frac{A}{r^5}[e_x({2x}^2-y^2-z^2)+e_y\left(3\mathrm{xy}\right)+e_z\left(3\mathrm{xz}\right)]$

$B_2=\frac{A}{r^5}[e_x\left(3\mathrm{xy}\right)+e_y({2y}^2-x^2-z^2)+e_2\left(3\mathrm{yz}\right)]$

其中，ω为旋转磁体角速度，且

$A=\frac{{\mathrm{\μ}}_0{M}_0{\mathrm{la}}^2}{4},r=\sqrt{x^2+y^2+z^2}$

上式中，μ₀为磁导率，M₀,l,a分别为旋转磁体的磁荷密度，长度和半径。

对于第m段旋转磁场数据的解算过程为：

（1）由旋转磁场数据sm提取总磁场B(t)={B_x(t),B_y(t),B_z(t)}。

（2）对|B(t)|取希尔伯特变换H[|B(t)|]，求取角度

$\mathrm{\φ}\left(t\right)=a\tan \left(\frac{H\left[\right|B\left(t\right)\left|\right]}{\left|B\left(t\right)\right|}\right)$

（4）求导引向量

L=(L_x,L_y,L_z)=B′₁×B′₂

其中，

$B_1^{\′}=\frac{1}{\mathrm{\π}}{\∫}_0^{2\mathrm{\π}}B\left(t\right)\cos \mathrm{\φ}\left(t\right)d\left(\mathrm{\ωt}\right)$

$B_2^{\′}=\frac{1}{\mathrm{\π}}{\∫}_0^{2\mathrm{\π}}B\left(t\right)\sin \mathrm{\φ}\left(t\right)d\left(\mathrm{\ωt}\right)$

（5）输出第m个测点位置（相对距离r、俯仰角方位角θ）

$\mathrm{\θ}=\arctan \left(\frac{L_y}{L_x}\right)$

$r=\sqrt[6]{\frac{A^2}{{\left|B_1^{\′}\right|}^2+{\left|B_2^{\′}\right|}^2}}$

记表示k时刻第m个测点位置，并作为第m个测量值输入Kalman滤波器9。

103、当2≤m≤M时，根据第m段姿态数据h_m和第m个测量值对第m-1次kalman滤波估计进行状态转移，得到第m-1次滤波估计的一步预测

即

或

其中，

记为第m次滤波估计，记为第m次滤波估计的协方差阵，记和分别为第m-1次滤波和协方差的一步预测。

如图3所示，k时刻目标物体实际位置由B点表示，而测量值为实际位置之前的第m个测点位置，以图中A点表示。由图中所示相对几何位置关系，可由A点位置几何外推得到B点位置分别以和替换A点和B点位置，并结合第m段姿态数据h_m，得到和的外推关系。

若和满足线性关系，则第m次滤波前，按如下的状态转移规则对m-1次滤波估计进行一步预测得到和

${\hat{X}}_{m/m-1}^{\left(k\right)}={\mathrm{\Φ}}_{m/m-1}^{\left(k\right)}{\hat{X}}_{m-1}^{\left(k\right)}$

$P_{m/m-1}^{\left(k\right)}={\mathrm{\Φ}}_{m/m-1}^{\left(k\right)}{P}_{m-1}^{\left(k\right)}{{\mathrm{\Φ}}_{m/m-1}^{\left(k\right)}}^T$

其中，Φ_k，m/m-1为系统转移矩阵，描述相对距离、方位角和井斜角间的几何关系。

由于和间不是线性关系，故本实施例中和由如下3组分量关系式表示：

其中，

104、将第m个测量值和第m-1次滤波估计的一步预测输入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m=1时，则将k-1时刻第M次滤波估计作为k时刻第0次滤波估计的一步预测；

状态转移完成后，进行第m次Kalman滤波，将第m次测量值和第m-1次滤波估计的一步预测输入Kalman滤波方程，得到第m次滤波估计及协方差阵

${\hat{X}}_m^{\left(k\right)}={\hat{X}}_{m/m-1}^{\left(k\right)}+B_m^{\left(k\right)}(Z_m^{\left(k\right)}-{\hat{X}}_{m/m-1}^{\left(k\right)})$

$P_m^{\left(k\right)}=(I-B_m^{\left(k\right)})P_{m/m-1}^{\left(k\right)}$

其中，滤波增益矩阵 $B_m^{\left(k\right)}=P_{m/m-1}^{\left(k\right)}{(P_{k,m/m-1}+R_{m/m-1}^{\left(k\right)})}^{-1},$ 为噪声协方差。

特别的，在进行第1次Kalman滤波前，将k-1时刻的第M次Kalman滤波估计作为k时刻第0次滤波估计的一步预测；同时以单位阵作为第0次滤波的协方差阵。记为

${\hat{X}}_0^{\left(k\right)}={\hat{X}}_M^{(k-1)}$

$P_0^{\left(k\right)}=I$

105、重复102、103和104步骤，并最终得到k时刻的第M次滤波估计，则k时刻目标物体的绝对空间坐标为：

$\begin{array}{c}{z}^{\left(k\right)}={\mathrm{\γ}}_M^{\left(k\right)}\sin {\mathrm{\θ}}_M^{\left(k\right)}\\ .\end{array}$

第m次Kalman滤波完成后，根据步骤102、103和104对下一段旋转磁场数据s_m+1进行处理，并最终得到k时刻的第M次滤波估计，则可以计算出k时刻目标物体的绝对空间坐标。

本发明所提供的动磁场下的滤波定位方法通过对采集的旋转磁场数据和姿态数据进行分段处理，将每段旋转磁场数据输入到旋转磁场模型进行解算获取每段旋转磁场数据的测量值，并结合相应地姿态数据对上一段旋转磁场数据的滤波估计实施状态转移，得到上一段旋转磁场数据滤波的一步预测，将一步预测和该段旋转磁场数据的测量值输入Kalman滤波器进行滤波，获取该段磁场数据的滤波估计，并依次向外逐段推算，最终计算出目标物体的当前位置，大幅降低了定位结果的方差，具有更高的定位精度，并解决了远场定位能力不足、甚至无法定位的缺陷，实现在目标物体连续运动过程中的实时测量定位，提高了动磁场定位的抗干扰能力。

实施例二

下面以具体实施例来说明本发明实施例中动磁场下的滤波定位方法实现对目标物体定位的过程。

如图5所示，磁传感器3置于空间点（0,0,0），目标物体a由图中A点向B点运动。其中AB段目标物体a运动速度为0.3642m/s，BC段为0.9702m/s，CD段为2.1852m/s，整个过程持续时间为100s。

图6a所示为本实施例中旋转磁场的分布示意图，为说明算法实施过程，以69.4s时的定位为例，真实位置（43.12,35.15,1.51），相对距离55.6520m，俯仰角1.5550°，方位角39.1858°。此时移位寄存器1中存储的磁场数据时间点范围64.0~69.4s，如图6b所示。时间戳中的时间节点为

……64.0,64.21,64.41,64.62,64.83,65.03,65.24,65.44,65.65,65.86,66.06,66.27,66.48,66.68,66.89,67.09,67.3,67.51,67.71,67.92,68.12,68.33,68.54,68.74,68.95,69.16,69.36，69.57……

根据时间戳在磁场数据中选取27个时间节点64.0,64.21,64.41,……，68.95,69.16,69.4，将64~69.4s的Bx、By和Bz数据分为26段。

将26段数据依次送入磁场模型解算并进行Kalman滤波，滤波结果如图7a、7b和7c所示，最终滤波输出为相对距离56.1121m，俯仰角1.5450°，方位角39.3710°，绝对空间坐标（43.3620,35.5812,1.5129）。从图7a、7b和7c中可以看出，Kalman滤波后输出的目标物体的相对距离、俯仰角和方位角随测点数增加不断收敛，与测量值的剧烈波动相比，Kalman滤波估计输出的方差降低到测量值方差的10%以下，具有更高的定位精度。

按照本发明实施例中动磁场下的滤波定位方法给出整个100s运动过程的定位结果，以相对距离为例，如图8a所示，图中细实线表示仅用旋转磁场模型测量而不加滤波的定位结果，波动十分剧烈，其方差随距磁源相对距离增加急剧增大，50m以外的定位精度已变得很差，而达到70m以后其定位结果无法采信。粗实线表示加入本发明的Kalman滤波的结果，由于其大幅降低结果方差，能够将定位结果收敛在一个可信的范围内，这一优势在远场定位时表现的尤为明显，弥补了传统磁场定位远场测量能力不足的缺陷。同时，图8b和图8c分别为图8a中50s和82.67s对应的虚线框内的局部放大图中，50s和82.67s的位置，其定位方差具有明显的分界。在此两处被测目标运动速度分别由0.3642m/s、0.9702m/s变化至0.9702m/s和2.1852m/s，被测目标运动速度的增加造成对磁场数据采集量不足，亦影响磁场模型测量的结果，而Kalman滤波结果较好的适应运动速度的变化，满足被测目标实时连续运动过程中的测量需求。

为更好的说明本发明所提供的动磁场下的滤波定位方法的抗干扰能力，以67.51~69.4s时对目标物体施加突发瞬时强干扰为例来说明，图9a所示为对应的旋转磁场分布示意图，从图9b和图9c中可以看出，相应地第18~26个测点由磁场模型解算的测量值无法采信，而Kalman滤波结果仍具有较好的收敛性质，滤波输出为相对距离57.2003m，俯仰角2.1642°，方位角39.0200°，仍具有较高的可信度，因而本发明所提供的动磁场下的滤波定位方法提高了动磁场定位的抗干扰能力。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (4)

1.一种动磁场下的滤波定位方法，其特征在于，包括：

100、在目标物体的运动过程中不断采集更新目标物体的旋转磁场数据和姿态数据并存储，同时根据预设规则生成一组时间节点，其中，旋转磁场数据包括x、y、z三个方向的磁场B_x、B_y、B_z及采样时间，姿态数据包括俯仰角方位角θ_s及采样时间；

101、k时刻，根据所述一组时间节点分别将存储的旋转磁场数据和姿态数据分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数；

102、将第m段旋转磁场数据s_m输入旋转磁场模型进行解算，得到第m个测量值其中k为k时刻，为k时刻第m个测点的相对距离，为k时刻第m个测点的俯仰角，为k时刻第m个测点的方位角；

103、当2≤m≤M时，根据第m段姿态数据h_m和第m个测量值对第m-1次Kalman滤波估计进行状态转移，得到第m-1次滤波估计的一步预测

即

或

其中，；

104、将第m次测量值和第m-1次滤波估计的一步预测输入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m＝1时，则将k-1时刻第M次滤波估计作为k时刻第0次滤波估计的一步预测；

105、重复102、103和104步骤，并最终得到k时刻的第M次滤波估计则k时刻目标物体的绝对空间坐标为：

$z^{\left(k\right)}={\mathrm{\γ}}_M^{\left(k\right)}{\mathrm{sin\θ}}_M^{\left(k\right)}.$

2.根据权利要求1所述的动磁场下的滤波定位方法，其特征在于，步骤100中根据预设规则生成一组时间节点具体为：

通过连接线与目标物体连接，所述连接线的长度每变化△L即设定一个时间节点。

3.根据权利要求1所述的动磁场下的滤波定位方法，其特征在于，步骤101具体为：

首先获取存储的旋转磁场数据的第一个采样时间t_A，找到与t_A最接近的时间节点t₀；

然后依次取t₀后面的时间节点t₁、t₂……t_M，并在存储的旋转磁场数据中依次获取与时间节点t₁、t₂……t_M相同的采样时间，直到找不到与时间节点t_M+1相同的采样时间，并以存储的旋转磁场数据的最后一个采样时间t_B替换t_M，记t_A-t₁段旋转磁场数据为s₁，t₁-t₂段旋转磁场数据为s₂，……,t_M-1-t_B段旋转磁场数据为s_M；

根据时间节点t_A、t₁、t₂……t_B将存储的姿态数据分为M段，记t_A-t₁段姿态数据为h₁，t₁-t₂段姿态数据为h₂，……,t_M-1-t_B段姿态数据为h_M。

4.一种实现权利要求1-3所述的动磁场下的滤波定位方法的装置，其特征在于，包括安装在目标物体上的旋转磁体和姿态传感器、位置已知并用于感应所述旋转磁体旋转磁场的磁传感器、第一移位寄存器、第二移位寄存器、数据分段器、时间戳、旋转磁场模型和Kalman滤波器；

其中，所述第一移位寄存器和第二移位寄存器分别用于存储所述磁传感器感应的旋转磁场数据和所述姿态传感器感应的目标物体的姿态数据并不断更新；

所述时间戳用于根据预设规则生成一组时间节点；

所述数据分段器用于根据所述一组时间节点将所述第一移位寄存器中的旋转磁场数据和所述第二移位寄存器中的姿态数据分别分为M段，分别记为s_m和h_m，其中，1≤m≤M，m、M为自然数，并将分段后的旋转磁场数据s_m依次输入所述旋转磁场模型，分段后的姿态数据h_m依次输入Kalman滤波器；

所述旋转磁场模型依次对每段旋转磁场数据s_m进行解算，并将解算结果作为第m个测量值输入Kalman滤波器；

当2≤m≤M时，所述Kalman滤波器首先结合第m段姿态数据h_m和第m个测量值对第m-1次滤波估计实施状态转移，得到第m-1次滤波估计的一步预测然后将第m-1次滤波估计的一步预测及第m个测量值带入Kalman滤波方程进行第m次Kalman滤波，得到第m次滤波估计其中，当m＝1时，则将k-1时刻第M次滤波估计作为k时刻第0次滤波估计的一步预测；直至得到k时刻的第M次滤波估计