CN113238291A - 基于磁性部件定位跟踪的地磁矢量实时估计与分离方法 - Google Patents

Abstract

本发明涉及基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，按照以下方法实现：通过磁传感器阵列获取对应磁性部件空间磁场分布的感应磁场信号；通过分离感应磁场信号中的地磁场矢量来获取磁性部件信号，将地磁场作为一个三维矢量G＝(Gx,Gy,Gz)实时建模，并在磁性部件定位跟踪过程中地磁场矢量作为待求解参数和磁性部件位姿信息通过定位跟踪算法一同求解，并通过最优化算法实时估计和分离磁传感器阵列测得的地磁分量。将磁传感器阵列测量点感应的地磁场信号建模为一个三维矢量并作为磁性部件定位跟踪过程中待求解变量与磁性部件的位姿参数等一同求解，从而实现地磁场矢量实时估计与分离，得到实现磁性部件定位跟踪性能的提升。

Description

基于磁性部件定位跟踪的地磁矢量实时估计与分离方法

技术领域

本发明涉及磁性部件定位跟踪技术领域，特别是基于磁性部件定位跟踪的地磁矢量实时估计与分离方法。

背景技术

磁性部件作为磁场信号源，通过磁传感器阵列得到其空间磁场分布，进而得到磁性部件与磁传感器阵列之间的相对位姿；具有无源(磁性部件不需要能源供给)、无线(磁性部件和磁传感器阵列无线连接且无光学阻挡问题)和装置简单等优点。中科院深圳先进院胡超团队、重庆大学皮喜田团队、上海交大颜国正团队和意大利比萨圣安娜大学PaoloDario团队等开展了深入研究，可以应用于胶囊内镜在人体内部的无线实时定位跟踪，中科院海西研究院戴厚德团队将其拓展应用于移动机器人的高精度停车/转向以及肺部肿瘤的实时跟踪等用途。

被实时定位跟踪的磁性部件可以是单个或者多个，磁传感器阵列中磁传感器总的轴数(即维数)大于被待求解的参数个数即可。一个磁性部件包含6维位姿参数及其磁性参数，地磁场矢量参数为3维。

地磁场在磁性部件的定位跟踪过程中是干扰磁场，需要从传感器测量信号中分离。但同样作为静磁场的永磁铁和地磁场的信号叠加，在实际应用中二者的分离困难较大。原因还包括在非屏蔽室地磁场广泛存在，由于环境中铁磁物质的干扰，地磁场的分布呈现一定的不规律性；同时地磁场作为3维矢量，传感器阵列的位姿变化将导致测量得到的地磁场三维分量发生变化。

地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。

美国PATH智能车研究团队提出平均法、峰谷识别法、双传感器差分测量法预估地磁场分量。平均法是对智能车运行区域不同点的地磁场信号进行多次重复测量并取其平均值作为地磁场数据，但是其假设前提是智能车运行区域的地磁场一致，在实际情况下很难满足。峰谷识别法是利用磁性部件信号垂直分量来识别两个磁性部件磁钉间的磁场信号最小值，并将其作为地磁场固定分量以供后续的传感器实测值进行删减。双传感器差分测量法是采用两个相距一定距离的磁传感器对来分别测量不同位置磁场大小，其中远端磁传感器测得的主要是地磁场信号，用近端磁传感器与远端磁传感器的测量值作差分运算就可得到有效磁场信号，但是该方法需要远端传磁感器远离磁性部件等干扰源且磁传感器对的地磁场相对一致。上海交大杨明团队通过判断磁尺数据一致性方法来获得地磁场信号强度，即没有磁性部件磁钉存在情况下测得的磁传感器数据作为地磁场信号。

上述方法在环境变化和外界干扰情况下无法及时进行修正，导致鲁棒性较差和精度较低。所以对环境磁场进行实时精准建模就非常必要，由于地磁场也可以和圆柱形磁性部件同样等效为磁偶极子，可以将地磁场分量3个参数作为求解参数，在磁性部件定位跟踪算法实现过程中可以通过最优化算法实时估计和分离磁传感器阵列测得的地磁分量。

发明内容

为解决上述问题，本发明的目的是提供基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，不需要在定位跟踪初始时刻采集地磁场信号(在通用方法中作为固定的地磁场信号强度从磁传感器阵列里面减去)且磁传感器阵列和磁性部件在定位跟踪中可以同时移动。

本发明实施例中采用以下方案实现：提供基于磁性部件定位跟踪的地磁矢量实时估计与分离方法,按照以下方法实现：

通过磁传感器阵列获取对应磁性部件空间磁场分布的感应磁场信号；

通过分离感应磁场信号中的地磁场矢量来获取磁性部件信号，将地磁场作为一个三维矢量G＝(Gx,Gy,Gz)实时建模，并在磁性部件定位跟踪过程中地磁场矢量作为待求解参数和磁性部件位姿信息通过定位跟踪算法一同求解，并通过最优化算法实时估计和分离磁传感器阵列测得的地磁分量。

本发明一实施例中，若所述磁性部件为单个，磁性部件的位姿求解函数为f(a,b,c,m,n,p,Gx,Gy,Gz)；若所述磁性部件为多个，磁性部件的位姿跟踪求解函数为f(a₁,b₁,c₁,m₁,n₁,p₁,a₂,b₂,c₂,m₂,n₂,p₂,...,a_M,b_M,c_M,m_M,n_M,p_M,G_x,G_y,G_z)，式中M为被定位跟踪的磁性部件个数。

本发明一实施例中，当被定位跟踪的磁性部件为单个时，在基于磁性部件的物体定位跟踪过程中，磁性部件及其空间磁场分布等效为磁偶极子模型，其位姿信息(a,b,c,m,n,p)^T作为拟求解参数代入f(a,b,c,m,n,p)，(a,b,c)^T为磁性部件的位置，H₀＝(m，n，p)^T为磁性部件的方向；在空间点

上由第l个磁传感器感应的磁感应强度：

式(1)中P_l＝(x_l-a，y_l-b，z_l-c)^T；

m²+n²+p²＝1；

为磁性部件的磁性系数；N为磁传感器的个数；

为磁性部件的归一化方向向量，且

被定位跟踪的磁性部件个数多于一个时，空间磁场分布表现为多个磁偶极子产生磁场的叠加，则M个磁性部件在第l个磁传感器上感应的磁场强度如下：

式(2)中R_lq表示第q个磁性部件中心点与第l个磁传感器中心点之间的距离；l＝1，2，3，..，N，其中N为磁传感器的个数；q＝1，2，3，..，M，其中M为被定位跟踪的磁性部件的个数；

通过磁传感器阵列实际测量值(B_sl)和磁性部件磁偶极子模型估计值(B_l)的差值，可以得到如下数据集：

结合磁性部件姿态约束条件可以得到以下最优化问题：

通过求解得到磁性部件的六维位姿信息(a，b，c，m，n，p)。

本发明一实施例中，当磁传感器阵列在平移和旋转时，输出的地磁场三轴分量G＝(G_x，G_y，G_z)将相应变化并表述为f(G_x，G_y，G_z)；将f(G_x，G_y，G_z)与磁性部件的位姿参数一同作为未知量进行求解；

磁传感器阵列中的第l个磁传感器测得的值为B_sl＝B_l+G+ε，(5)

式(5)中ε为传感器测量噪声；

其中M为被定位跟踪的磁性部件的个数；l＝1，2，3，..，N；N为磁传感器的个数；从而可以由上式得到地磁场矢量和磁性部件位置信息；

当B_T未知时，也可以作为未知参数由上式求解f(a，b，c，m，n，p，G_x，G_y，G_z)得到；对于单目标，总的待求解参数为F＝[a，b，c，m，n，p，G_x，G_y，G_z]^T；对于多目标，每个磁性部件的位姿参数均作为求解参数进行计算；假设第1个磁传感器的实际观测值B_el＝(B_elx，B_ely，B_elz)，期望观测值为B_sl，实际观测值和期望观测值之差为

所以，传感器阵列三轴总的误差如下：

结合磁性部件姿态约束条件可以得到如下最优化问题：

通过最优化算法或者解析法求解可得各个磁性部件的位姿以及实时地磁场矢量，即求解f(a₁，b₁，c₁，m₁，n₁，p₁，a₂，b₂，c₂，m₂，n₂，p₂，...，a_M，b_M，c_M，m_M，n_M，p_M，G_x，G_y，G_z)。

本发明的有益效果：本发明提供基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，本发明方法将地磁场作为一个三维矢量实时建模，在基于磁性部件的定位跟踪过程中地磁场矢量三个参数作为待求解参数和一个或者多个磁性部件位姿信息一同求解(也可以包含磁性部件磁性系数BT)，从而实现了在基于磁性部件的定位跟踪过程中地磁场矢量的实时估计与分离，极大地提高了磁性部件的定位跟踪的精度和鲁棒性，进而提高了磁性部件定位跟踪的实用性。地磁信号的影响是磁性部件定位跟踪的一个难点，特别是在磁性部件和磁传感器阵列较远距离时，由于磁性部件信号衰减比较大，磁性部件定位跟踪性能受影响特别大。本方法较好地破解了这个难题。

附图说明

图1是本发明单个磁性部件(单目标)定位跟踪参数示意图。

图2是本发明多个磁性部件(多目标)定位跟踪参数示意图；

图3是本发明的地磁场矢量的实时估计与分离方法示意图；其中磁性部件磁场分布模型(磁偶极子模型等)和三维地磁场矢量模型进行叠加作为模型预测值，故磁传感器阵列测量值和模型预测值之间的最小化方程加入三维地磁场矢量三个未知参数，通过计算求解可以同时得到实时的地磁场矢量和各个磁性部件的位姿。

具体实施方式

下面结合附图对本发明做进一步说明。

请参阅图1至图3，本发明提供基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，按照以下方法实现：

通过磁传感器阵列获取对应磁性部件空间磁场分布的感应磁场信号；

通过分离感应磁场信号中的地磁场矢量来获取磁性部件信号，将地磁场作为一个三维矢量G＝(Gx,Gy,Gz)实时建模，并在磁性部件定位跟踪过程中地磁场矢量作为待求解参数和磁性部件位姿信息通过定位跟踪算法一同求解，并通过最优化算法实时估计和分离磁传感器阵列测得的地磁分量；

若所述磁性部件为单个，磁性部件的位姿求解函数为f(a,b,c,m,n,p,Gx,Gy,Gz)；若所述磁性部件为多个，磁性部件的位姿跟踪求解函数为f(a₁,b₁,c₁,m₁,n₁,p₁,a₂,b₂,c₂,m₂,n₂,p₂,...,a_M,b_M,c_M,m_M,n_M,p_M,G_x,G_y,G_z)，式中M为被定位跟踪的磁性部件个数；

当被定位跟踪的磁性部件为单个时，在基于磁性部件的物体定位跟踪过程中，磁性部件及其空间磁场分布等效为磁偶极子模型，其位姿信息(a,b,c,m,n,p)^T作为拟求解参数代入f(a,b,c,m,n,p)，(a,b,c)^T为磁性部件的位置，H₀＝(m,n,p)^T为磁性部件的方向；在空间点(x_l,y_l,z_l)^T上由第l个磁传感器感应的磁感应强度：

式(1)中P_l＝(x_l-a,y_l-b,z_l-c)^T；

m²+n²+p²＝1；B_T为磁性部件的磁性系数；N为磁传感器的个数；

为磁性部件的归一化方向向量，且

被定位跟踪的磁性部件个数多于一个时，空间磁场分布表现为多个磁偶极子产生磁场的叠加，则M个磁性部件在第l个磁传感器上感应的磁场强度如下：

式(2)中R_lq表示第q个磁性部件中心点与第l个磁传感器中心点之间的距离；l＝1,2,3,..,N，其中N为磁传感器的个数；q＝1,2,3,..,M，其中M为被定位跟踪的磁性部件的个数；

通过磁传感器阵列实际测量值(B_sl)和磁性部件磁偶极子模型估计值(B_l)的差值，可以得到如下数据集：

结合磁性部件姿态约束条件可以得到以下最优化问题：

通过求解得到磁性部件的六维位姿信息(a,b,c,m,n,p)。在磁性部件远场处受信噪比影响，跟踪精度受到较大影响。在跟踪多个磁性部件目标时，位姿参数数量和算法复杂度将随着磁性部件个数成倍数增加。综上，对于基于磁偶极子模型的磁性部件跟踪，其跟踪精度与跟踪个数、距离和环境噪声等影响因素有关；

所述磁传感器阵列的测量信号由近似匀强磁场的地磁场矢量信号和近似磁偶极子模型分布的磁性部件信号线性叠加而成，即磁传感器测量值大小取决于地磁场矢量和磁铁位姿矢量；地球磁场像一个条形磁性部件一样由磁南极指向磁北极。在磁极点处磁场和当地的水平面垂直，在赤道磁场和当地的水平面平行，所以在北半球磁场方向倾斜指向地面；用来衡量磁感应强度大小的单位是Tesla或者Gauss，地磁场强度随着地理位置的不同大致在0.4～0.6Gauss之间；地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面平行的分量和一个与当地水平面垂直的分量。如果保持三轴磁传感器和测量点水平面平行，那么三轴磁传感器的三个轴就和地磁场矢量的三个分量(H_x,H_y,H_z)对应；当磁传感器阵列在平移和旋转时，输出的地磁场三轴分量G＝(G_x,G_y,G_z)将相应变化并表述为f(G_x,G_y,G_z)；将f(G_x,G_y,G_z)与磁性部件的位姿参数一同作为未知量进行求解；

磁传感器阵列中的第l个磁传感器测得的值为B_sl＝B_l+G+ε,(5)

式(5)中ε为传感器测量噪声；

其中M为被定位跟踪的磁性部件的个数；l＝1,2,3,..,N；N为磁传感器的个数；从而可以由上式得到地磁场矢量和磁性部件位置信息；

当B_T未知时，也可以作为未知参数由上式求解f(a,b,c,m,n,p,G_x,G_y,G_z)得到；对于单目标，总的待求解参数为F＝[a，b,c,m,n,p,G_x,G_y,G_z]^T；对于多目标，每个磁性部件的位姿参数均作为求解参数进行计算；假设第l个磁传感器的实际观测值B_el＝(B_elx,B_ely,B_elz)，期望观测值为B_sl,实际观测值和期望观测值之差为

所以，传感器阵列三轴总的误差如下：

结合磁性部件姿态约束条件可以得到如下最优化问题：

通过最优化算法或者解析法求解可得各个磁性部件的位姿以及实时地磁场矢量，即求解f(a₁,b₁,c₁,m₁,n₁,p₁,a₂,b₂,c₂,m₂,n₂,p₂,...,a_M,b_M,c_M,m_M,n_M,p_M,G_x,G_y,G_z)。

本发明实施例中，本发明方法所使用的磁性部件定位跟踪系统由磁性部件(一个或者多个)和磁传感器阵列，以及数据处理模块组成；所述磁性部件作为无线的静磁场信号源；磁传感器阵列的数据采集以后传输到所述数据处理平台进行预处理、运算、存储、显示或者向外部传输，用于工业和医疗等用途；所述数据处理平台可以为单片机、高性能微处理器或者计算机等；均为现有设备。

本发明具有以下工作原理：

本发明通过将磁传感器阵列测量点感应的地磁场信号建模为一个三维矢量并作为磁性部件定位跟踪过程中待求解变量与磁性部件的位姿参数等一同求解，从而实现地磁场矢量实时估计与分离，得到实现磁性部件定位跟踪性能的提升。

三维地磁场矢量G＝(G_x,G_y,G_z)作为变量用于磁性部件位姿和磁性系数等信息一同求解，设定为磁传感器阵列中不同磁传感器位置点的地磁场矢量相同，即近似匀强磁场(即磁传感器阵列中心点的地磁场矢量)。

以上所述仅为本发明的较佳实施例，不能理解为对本申请的限制，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (4)

1.基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，其特征在于：按照以下方法实现：

通过磁传感器阵列获取对应磁性部件空间磁场分布的感应磁场信号；

通过分离感应磁场信号中的地磁场矢量来获取磁性部件信号，将地磁场作为一个三维矢量G＝(Gx,Gy,Gz)实时建模，并在磁性部件定位跟踪过程中地磁场矢量作为待求解参数和磁性部件位姿信息通过定位跟踪算法一同求解，并通过最优化算法实时估计和分离磁传感器阵列测得的地磁分量。

2.根据权利要求1所述的基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，其特征在于：若所述磁性部件为单个，则磁性部件的位姿求解函数为f(a,b,c,m,n,p,Gx,Gy,Gz)；若所述磁性部件为多个，则磁性部件的位姿跟踪求解函数为f(a₁,b₁,c₁,m₁,n₁,p₁,a₂,b₂,c₂,m₂,n₂,p₂,...,a_M,b_M,c_M,m_M,n_M,p_M,G_x,G_y,G_z)，式中M为被定位跟踪的磁性部件个数。

3.根据权利要求2所述的基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，其特征在于：当被定位跟踪的磁性部件为单个时，在基于磁性部件的物体定位跟踪过程中，磁性部件及其空间磁场分布等效为磁偶极子模型，其位姿信息(a,b,c,m,n,p)^T作为拟求解参数代入f(a,b,c,m,n,p)，(a,b,c)^T为磁性部件的位置，H₀＝(m,n,p)^T为磁性部件的方向；在空间点(x_l，y_l，z_l)^T上由第l个磁传感器感应的磁感应强度为：

式(1)中P_l＝(x_l-a，y_l-b，z_l-c)^T，

m²+n²+p²＝1，B_T为磁性部件的磁性系数；N为磁传感器的个数；

为磁性部件的归一化方向向量，且

被定位跟踪的磁性部件个数多于一个时，空间磁场分布表现为多个磁偶极子产生磁场的叠加，则M个磁性部件在第l个磁传感器上感应的磁场强度如下：

式(2)中R_lq表示第q个磁性部件中心点与第l个磁传感器中心点之间的距离；l＝1,2,3,..,N，其中N为磁传感器的个数；q＝1,2,3,..,M，其中M为被定位跟踪的磁性部件的个数；

通过磁传感器阵列实际测量值(B_sl)和磁性部件磁偶极子模型估计值(B_l)的差值，可以得到如下数据集：

结合磁性部件姿态约束条件可以得到以下最优化问题：

通过求解得到磁性部件的六维位姿信息(a,b,c,m,n,p)。

4.根据权利要求3所述的基于磁性部件定位跟踪的地磁矢量实时估计与分离方法，其特征在于：

当磁传感器阵列在平移和旋转时，输出的地磁场三轴分量G＝(G_x,G_y,G_z)将相应变化并表述为f(G_x,G_y,G_z)；将f(G_x,G_y,G_z)与磁性部件的位姿参数一同作为未知量进行求解；

磁传感器阵列中的第l个磁传感器测得的值为B_sl＝B_l+G+ε,(5)

式(5)中ε为传感器测量噪声；

其中M为被定位跟踪的磁性部件的个数；l＝1,2,3,..,N；N为磁传感器的个数；从而可以由上式得到地磁场矢量和磁性部件位置信息；

当B_T未知时，也可以作为未知参数由上式求解f(a,b,c,m,n,p,G_x,G_y,G_z)得到；

对于单目标，总的待求解参数为F＝[a,b,c，m，n，p，G_x，G_y，G_z]^T；对于多目标，每个磁性部件的位姿参数均作为求解参数进行计算；假设第l个磁传感器的实际观测值B_el＝(B_elx，B_ely，B_elz)，期望观测值为B_sl，实际观测值和期望观测值之差为

所以，传感器阵列三轴总的误差如下：

结合磁性部件姿态约束条件可以得到如下最优化问题：

通过最优化算法或者解析法求解可得各个磁性部件的位姿以及实时地磁场矢量，即求解f(a₁,b₁,c₁,m₁,n₁,p₁,a₂,b₂,c₂,m₂,n₂,p₂,...,a_M,b_M,c_M,m_M,n_M,p_M,G_x,G_y,G_z)。

Images