一种基于非线性滤波的磁定位方法技术

本发明专利技术公开了一种基于非线性滤波的磁定位方法，属于磁定位技术领域。所述定位方法包括以下步骤：A.设置磁传感器观测网，获得所有磁传感器的空间坐标；B.通过各个磁传感器测量得到磁性目标内置永磁体的磁通量密度，用当前统计模型建立以磁性目标位置和方向为变量的状态方程，用毕奥

【技术实现步骤摘要】
一种基于非线性滤波的磁定位方法


[0001]本专利技术涉及一种基于非线性滤波的磁定位方法，属于磁定位


技术介绍

[0002]近年来，无线定位技术已经得到了广泛的应用。近距离，尤其是在非视距场景下的高精度、低成本的定位技术有极大的应用价值。胶囊内窥镜对于胃肠道病变的检查因其具有无创伤等优点而得到广泛应用。其准确实时定位可以帮助医生快速获取病灶部位的准确信息，以辅助医生对患者进行诊断和治疗。
[0003]目前，现有技术之一，电磁定位在基于到达时间(ToA)、接收信号强度(RSS)、到达时间差(TDoA)等技术方法方面的难度主要在于：人体的结构复杂，不同器官具有差异较大的介电常数，从而对电磁波的传播速度等有较大影响，使得准确的信号模型建立比较困难，所以这方面目前通常定位精度较低。
[0004]现有技术之二，基于磁定位的技术。通常是通过Levenberg
‑
Marquardt(LM)算法，或者粒子群优化等算法求解非线性误差函数得到磁性目标的位置和方向。由于在计算过程中存在迭代，使得实时性较差，同时抗传感器量测噪声性能较差。

技术实现思路

[0005]本专利技术提供一种基于非线性滤波的磁定位技术，实现实时、连续和高精度地定位跟踪磁性目标，同时具有良好的抗传感器量测噪声性能。
[0006]为了达到上述目的，本专利技术提供一种基于非线性滤波的磁定位技术，包含以下步骤：
[0007]步骤S1、用下式表示的当前统计模型对磁性目标建立状态方程：
[0008][0009]其中ψ
k+1
和ψ
k
分别表示磁性目标k+1和k时刻状态向量，ψ
k
＝[a
k
,b
k
,c
k
,m
k
,n
k
,p
k
]T
，[a
k
,b
k
,c
k
]T
表示磁性目标k时刻的位置，[m
k
,n
k
,p
k
]T
表示磁性目标k时刻的方向(这里的位置和方向指可磁偶极子模型化永磁体的位置和方向，下同)；G
k
表示磁性目标k时刻的控制矩阵；表示磁性目标机动加速度平均值；w
k
表示k时刻的离散时间白噪声。
[0010]w
k
的方差为为实对称阵。
[0011][0012][0013][0014][0015][0016][0017][0018]其中T为采样周期；α为自相关时间常数，与T有关；
[0019][0020]步骤S2、测量方程由下式表示：
[0021]B'
l,k+1
＝h
l
(ψ
k+1
)+V
l,k+1
[0022]B'
l,k+1
＝[B'
lx,k+1
B'
ly,k+1
B'
lz,k+1
]T
为k+1时刻第l个传感器在参考直角坐标系x，y，z方向上的磁通量密度测量值，l＝1,2,
…
,N，N为传感器的个数；
[0023][0024]h
l
(ψ
k+1
)是k+1时刻第l个磁传感器根据毕奥
‑
萨伐尔定律计算的在参考直角坐标系x，y，z方向的磁通量密度理论值；x
l
,y
l
,z
l
是第l个磁传感器在参考直角坐标系下的x，y，z坐标；R
l,k+1
是k+1时刻第l个磁传感器到磁性目标的距离；V
l,k+1
是k+1时刻第l个传感器的离散时间白噪声；μ是磁导率，M是磁化强度。
[0025]步骤S3、所用的平方根无迹卡尔曼滤波步骤如下：
[0026]滤波系统初始化，
[0027]chol(
·
)为Cholesky分解。
[0028]令Sigma点为：
[0029][0030]其中S
k|k
是协方差P
k|k
的Cholesky分解，即λ＝α2(n+κ)
‑
n，α是很小的正数，可取10
‑4≤α≤1，κ＝3
‑
n。
[0031]Sigma点的权值为
[0032]其中β取值与X的分布形式有关，对于正态分布，β＝2为最优值。
[0033]步骤S4、Sigma点经过状态方程的传递得到一步预测：
[0034]状态预测估值为
[0035]状态预测协方差平方根由下式计算：
[0036][0037]其中qr(
·
)是QR分解。
[0038][0039]其中cholupdate(
·
)是Cholesky一阶更新，为标准的Matlab指令。
[0040]步骤S5、量测预测Sigma点为
[0041]量测预测估值为
[0042]量测预测协方差平方根：
[0043][0044][0045]量测预测和状态预测互协差：
[0046]步骤S6、增益更新为：
[0047]状态更新：
[0048]其中B'
l,k+1
是第l个磁传感器的量测值。
[0049]状态协方差的平方根更新：
[0050]标准的卡尔曼滤波只适用于状态和量测方程均为线性的情况，无迹卡尔曼滤波可用于状态和量测方程均为非线性的情况，其以协方差矩阵递推更新，但在状态变量维数较高时，协方差矩阵平方根运算的累积误差会对Sigma点的精度造成较大影响，从而使得滤波的数值稳定性降低。
[0051]所以本专利技术选取了平方根无迹卡尔曼滤波，该算法以协方差矩阵的平方根递推更新，数值计算过程具有稳定的特性，并可保证协方差矩阵的半正定性；当前统计模型对于机动目标的跟踪具有优良的性能，可以用来跟踪磁性目标的运动状态。
附图说明
[0052]图1为本专利技术的流程示意图；
[0053]图2示出了本专利技术所涉及的一种基于非线性滤波的磁定位方法用于胶囊内窥镜系统的示意图。
[0054]图3为本专利技术的位置和角度定位跟踪仿真结果。
[0055]符号说明：
[0056]1…
被检体，2
…
磁性目标，3
…
永磁体，4
…
磁传感器，5
…
信号处理终端。
具体实施方式
[0057]下面结合图1，阐述本专利技术的实现步骤如下：
[0058]步骤1，开始阶段，读取传感器坐标，确定参考直角坐标系，本专利技术实施例中周期同步采样频率100Hz。
[0059]步骤2，以当前统计模型建立磁性目标的运动模型，构造状态方程本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于非线性滤波的磁定位方法，其特征在于：采用磁传感器观测网实现磁性目标的精确定位跟踪，包括如下步骤：S1、读取磁传感器观测网中各磁传感器在参考直角坐标系中的位置，以周期同步采样的方式读取磁性目标内置永磁体在各磁传感器处的磁通量密度测量值；S2、以磁性目标的位置和方向参数为状态变量，采用当前统计模型建立状态方程；以各磁传感器测得的磁通量密度为测量输出，根据毕奥
‑
萨伐尔定律计算的磁通量密度值为理论值，建立量测方程。2.根据权利要求1所述的一种基于非线性滤波的磁定位方法，其特征在于状态空间方程的建立包含以下步骤：S201、预设磁性目标加速度极值，机动时间常数，采样频率；S202、由下式的当前统计模型对磁性目标建立状态方程：其中F
k
是状态转移矩阵；ψ
k+1
和ψ
k
分别表示磁性目标k+1和k时刻的状态向量，ψ
k
＝[a
k
,b
k
,c
k
,m
k
,n
k
,p
k
]
T
，[a
k
,b
k
,c
k
]
T
表示磁性目标k时刻的位置，[m
k
,n
k
,p
k
]
T
表示磁性目标k时刻的方向(这里的位置和方向指可磁偶极子模型化永磁体的位置和方向)；G
k
表示磁性目标k时刻的控制矩阵；表示磁性目标机动加速度平均值；w
k
表示k时刻的离散时间白噪声；S203、由下式建立磁性目标的量测方程B
′
l,k+...

【专利技术属性】
技术研发人员：李海宁，
申请(专利权)人：李海宁，
类型：发明
国别省市：