CN109740117B - 一种鲁棒且快速的磁定位算法 - Google Patents

Abstract

本发明公开了一种鲁棒且快速的磁定位算法。SDM通过在解空间采样来学习广义梯度，避免了海森矩阵和雅可比矩阵的计算。TSDM使用奇异值特性修改SDM，沿着磁场强度差值最大的方向学习广义梯度，从而提高了定位精度。ITSDM将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点，这样训练时学习到的广义梯度更加接近真实的广义梯度，进一步提高了定位精度。本发明使用线性法快速估计磁极子参数的初值，然后借助改进后的监督下降法，即：奇异值截断SDM(TSDM)和迭代TSDM(ITSDM)对初始估计进行调整，实现鲁棒且快速的磁极子定位。此磁极子定位算法噪声抵御能力强，且计算复杂度更低。

Description

一种鲁棒且快速的磁定位算法

技术领域

本发明涉及无线磁极子定位技术领域，尤其涉及一种鲁棒且快速的磁定位算法。

背景技术

目前，无线磁极子定位技术被广泛地应用于舌部运动参数解码、内窥胶囊跟踪，计算机辅助手术等领域。该技术建立磁极子磁场模型拟合实测磁场数据，通过最小化拟合误差来估计磁极子的位置和角度。

现有技术之一：线性法，直接求磁极子欧拉方程的闭集解，计算复杂度低，但容易受噪声的影响，误差较大，例如：胡超等人利用磁通量密度(MFD)和极子至传感器矢量的叉乘与磁矩正交的性质，建立线性方程组，定位磁极子。

现有技术之二：非线性法，使用Levenberg-Marquardt，粒子群，Nelder-Mead和Powell等启发式优化方法,迭代搜索最佳磁极子参数，定位精度较高，但计算量较大，而且对初值敏感。

现有技术之三：复合法，先使用线性法快速估计磁极子参数的初值，然后借助非线性法对初始估计进行调整，一方面减轻了非线性优化的计算负荷，另一方面提高了定位精度，降低了陷入局部极值的风险，例如胡超等人将线性方法与Levenberg-Marquardt算法相结合，应用于胃肠道药片追踪。

目前以上这些方法主要用于计算资源较为充足，测量噪声小的固定设备，不适用于计算资源较为有限、测量噪声较大的便携式系统。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是提供一种鲁棒且快速的磁定位算法，首先使用线性法快速估计磁极子参数的初值，然后借助改进后的监督下降法(Supervised Descent Method,SDM)，即：奇异值截断SDM(TSDM)和迭代TSDM(ITSDM)对初始估计进行调整，实现鲁棒且快速的磁极子定位。此磁极子定位算法噪声抵御能力强，且计算复杂度更低。

为实现上述目的，本发明提供了一种鲁棒且快速的磁定位算法，包括以下步骤：

步骤1：令圆柱小磁铁的厚度为l，直径为d，剩磁为Br，位置为a＝[a_x,a_y,a_z]，θ和γ分别为小磁铁的仰角和旋转角，点s＝[s_x,s_y,s_z]与小磁铁的距离远大于l和d，则在s点测量的磁通量密度可用磁极子模型欧拉方程近似表达为：

其中m＝[m·sin(θ)cos(γ),m·sin(θ)sin(γ),m·cos(θ)]为磁极子的磁矩向量，m＝πB_rd²l/(4μ₀)；

步骤2：使用线性法直接求磁极子欧拉方程的闭集解，快速估计磁极子参数的初值；

步骤3：使用奇异值截断SDM、TSDM以及迭代TSDM、ITSDM对初始估计进行调整，实现鲁棒且快速的磁极子定位，已知磁极子定位目标函数：

其中x∈R⁵为待优化解，对应于磁极子的位置和仰角，B(x)为磁场模型函数，y表示磁场测量值，针对(2),基于梯度的迭代求解公式为：

其中α为更新步长，J_B为雅可比矩阵，A为缩放因子，SDM通过对解空间采样建立训练集，学习广义梯度

使得：

其中k＝0,1,2,...,T为迭代步数，

表示第i个采样点(亦称为锚点),X_k＝x_k1^T∈R^5×m；

c为磁传感器总数，在训练阶段，当k＝0时,R₀可以通过求解线性最小二乘问题获得：

其中ΔX＝x₀1^T-X_*,x₀为初始估计解,

(5)的解析解为：

其中

表示Ψ₀的伪逆运算，于是x₁＝x₀-R₀Ψ₀，继而推广到k≥1的情况有：

如此逐步迭代学习一系列的{R_k},直到

收敛，在测试阶段，SDM直接利用训练得到的广义梯度{R_k}逐步更新x，实现目标函数(2)的最小化，更新公式如下：

x_k＝x_k-1-R_k[B(x_k-1)-y] (8)

其中，y表示磁场测量值，y受到加性噪声n的影响时，x_k的更新误差可以表示为

具体的，所述

采取奇异值截断方法计算得到，丢弃较小奇异值所对应的成分，然后沿着磁场强度最大差异的方向学习R_k，Ψ_k的SVD可以表示为：

Ψ_k＝U∑V^T (9)

其中

为正交矩阵，假设样本点数m≥3c，rank(Ψ_k)＝n，n≤3c，则

可以表示为：

其中Λ_n×n＝diag(λ₁,λ₂,...,λ_n)，λ₁≥λ₂≥...≥λ_n是非零奇异值，因此Ψ_k可以表示为：

其中，奇异向量u_i和v_i分别是u和v的第i列，奇异值λ_i是第i个分量的尺度因子，对应于最大奇异值λ的奇异向量u_i表示Ψ_k最大方差的方向。

具体的，TSDM的广义梯度可以如下表示：

ITSDM将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点。

本发明的有益效果是：

(1)SDM通过在解空间采样来学习广义梯度，避免了海森矩阵和雅可比矩阵的计算，减少了计算时间。

(2)TSDM使用奇异值特性修改SDM，磁场强度差值最大的方向学习广义梯度，从而提高了定位精度。

(3)ITSDM将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点，这样训练时学习到的广义梯度更加接近真实的广义梯度，进一步提高了定位精度。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的传感器的分布图。

图2是本发明的不同定位算法在不同信噪比下的定位精度和计算时间对比图。

具体实施方式

磁极子定位问题本质上是一个非线性最小二乘问题，现有技术大多采用LM算法来实现磁定位，而LM算法本身是基于二阶梯度的寻优方法，它通过计算代价函数的海森矩阵的逆来实现迭代优化，在噪声的影响下，当海森矩阵不可逆时，LM需要不断调整海森矩阵主对角线的加强因子，这样降低了算法的收敛速度。针对该问题，2014年Xiong等人提出了SDM，通过在解空间采样学习广义梯度，避免了海森矩阵和雅可比矩阵的计算，收敛快。因此本发明采用SDM来实现快速磁极子定位。在训练阶段，SDM在当前解的周围随机生成采样点，并使用欧拉方程计算其理论磁场强度，然后根据当前解的磁场强度与采样点的磁场强度之间的差值来学习广义梯度。在测试阶段，SDM通过应用训练得到的广义梯度来减少当前解的磁场强度与实际测量的磁场强度之间的差异，最终求得磁极子的位置。但是，SDM所求得的解有可能不在最优解的附近，并且实际测量的磁场强度通常会包含噪声。因此在训练阶段中，当前解周围生成的采样点及其理论磁场强度可能是对最优解和实际测量磁场强度的有偏估计，这会严重降低测试阶段SDM的泛化能力。为了解决这个问题，本发明提出了TSDM和ITSDM。TSDM使用SVD来计算采样点和当前解之间的磁场强度差的伪逆，并丢弃对应于较小奇异值的分量。通过这种方式，TSDM可以沿着磁场强度差值最大的方向学习广义梯度，从而提高了定位精度。而ITSDM则将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点，这样训练时学习到的广义梯度更加接近真实的广义梯度，进一步提高了定位精度。

因此，本发明提供了一种鲁棒且快速的磁定位算法，包括以下步骤：

步骤1：假设本发明中的圆柱小磁铁的厚度为l，直径为d，剩磁为Br，位置为a＝[a_x,a_y,a_z]。θ和γ分别为小磁铁的仰角和旋转角。点s＝[s_x,s_y,s_z]与小磁铁的距离远大于l和d。因此，在s点测量的磁通量密度可用磁极子模型欧拉方程近似表达为：

其中m＝[m·sin(θ)cos(γ),m·sin(θ)sin(γ),m·cos(θ)]为磁极子的磁矩向量，m＝πB_rd²l/(4μ₀)。

步骤2：使用线性法直接求磁极子欧拉方程的闭集解，快速估计磁极子参数的初值。

步骤3：使用奇异值截断SDM(TSDM)和迭代TSDM(ITSDM)对初始估计进行调整，实现鲁棒且快速的磁极子定位。已知磁极子定位目标函数：

其中x∈R⁵为待优化解，对应于磁极子的位置和仰角，B(x)为磁场模型函数，y表示磁场测量值。针对(2),基于梯度的迭代求解公式为：

其中α为更新步长，J_B为雅可比矩阵，A为缩放因子：A＝I对应于一阶梯度法，收敛速度较慢；

(即海森矩阵的逆)对应于二阶梯度法，收敛速度较快，但是单步更新计算量较大。与它们不同，SDM通过对解空间采样建立训练集，学习广义梯度

使得：

其中k＝0,1,2,...,T为迭代步数，

表示第i个采样点(亦称为锚点),X_k＝x_k1^T∈R^5×m；

c为磁传感器总数。在训练阶段，当k＝0时,R₀可以通过求解线性最小二乘问题获得：

其中ΔX＝x₀1^T-X_*,x₀为初始估计解,

(5)的解析解为：

其中

表示Ψ₀的伪逆运算。于是x₁＝x₀-R₀Ψ₀。继而推广到k≥1的情况有：

如此逐步迭代学习一系列的{R_k},直到

收敛。在测试阶段，SDM直接利用训练得到的广义梯度{R_k}逐步更新x，实现目标函数(2)的最小化，更新公式如下：

x_k＝x_k-1-R_k[B(x_k-1)-y] (8)

值得注意的是：当磁场测量值y受到加性噪声n的影响时，x_k的更新误差可以表示为

为了减小更新误差，提高SDM磁定位算法的鲁棒性，本发明采取奇异值截断方法计算

丢弃较小奇异值所对应的成分，然后沿着磁场强度最大差异的方向学习R_k。Ψ_k的SVD可以表示为：

Ψ_k＝U∑V^T (9)

其中

为正交矩阵。假设样本点数m≥3c，rank(Ψ_k)＝n，n≤3c，则

可以表示为：

其中Λ_n×n＝diag(λ₁,λ₂,...,λ_n)，λ₁≥λ₂≥...≥λ_n是非零奇异值。因此Ψ_k可以表示为：

其中，奇异向量u_i和v_i分别是u和v的第i列。奇异值λ_i是第i个分量的尺度因子。对应于最大奇异值λ的奇异向量u_i表示Ψ_k最大方差的方向。而磁场强度差异最大的方向，具有较强的抗加性噪声干扰能力。因此，本发明使用奇异值截断：

其中r＜n。选择一个较小的r可以提高系统的鲁棒性，但它会丢弃更多的分量，从而降低TSDM的收敛速度。因此，本发明选择尽可能小的r，同时实现

则伪逆矩阵

可以通过如下计算：

TSDM的广义梯度可以如下表示：

ITSDM则将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点，这样训练时学习到的广义梯度更加接近真实的广义梯度，进一步提高了定位精度。

为了证明本发明提出的TSDM和ITSDM算法的有效性，特设置了仿真实验。

本发明采用16个传感器做仿真实验，将16个传感器均匀地放置在尺寸为0.24m×0.24m的方形平面上，其分布位置如图1所示。在x∈[-0.12,0.12]，y∈[-0.12,0.12]，z∈[0.06,0.25]，

范围内按均匀分布生成200个随机测试点。小磁铁的直径、长度和剩磁分别设定为d＝6mm，l＝1.25mm，Br＝1.48×10⁶G。

为了评价定位方法的鲁棒性，本发明在理论磁场强度中加入不同信噪比的高斯噪声，分别为10dB、15dB、20dB、25dB和30dB。SNR计算方法为：

SNR＝10log₁₀(P_s/P_n) (15)

其中

是平均信号能量，P_n＝P_s/10^SNR/10是平均噪声能量。在每个测试点上添加的噪声为：

其中

是服从正态分布的归一化随机向量。

本发明采用[4]中提出的位置E_p和角度E_o的平均误差来评估定位精度：

其中a_t,i和a_s,i是第i个测试点的测量和估计位置，h_t,i和h_s,i是第i个测试点的测量和估计的磁矩，n是测试点的个数。

实验结果如图2所示，由实验结果可知，LIN+TSDM、LIN+ITSDM的鲁棒性都要比LIN+LM好，并且花费的计算时间也要少，也就更适用于计算资源较为有限、测量噪声较大的便携式系统。因此，本发明提出的算法是可行的。

综上，本发明的有益效果是：

(1)SDM通过在解空间采样来学习广义梯度，避免了海森矩阵和雅可比矩阵的计算，减少了计算时间。

(2)TSDM使用奇异值特性修改SDM，磁场强度差值最大的方向学习广义梯度，从而提高了定位精度。

(3)ITSDM将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点，这样训练时学习到的广义梯度更加接近真实的广义梯度，进一步提高了定位精度。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (1)

1.一种鲁棒且快速的磁定位算法，其特征在于，包括以下步骤：

步骤1：令圆柱小磁铁的厚度为l，直径为d，剩磁为Br，位置为a＝[a_x,a_y,a_z]，θ和γ分别为小磁铁的仰角和旋转角，点s＝[s_x,s_y,s_z]与小磁铁的距离远大于l和d，则在s点测量的磁通量密度可用磁极子模型欧拉方程近似表达为：

其中m＝[m·sin(θ)cos(γ),m·sin(θ)sin(γ),m·cos(θ)]为磁极子的磁矩向量，m＝πB_rd²l/(4μ₀)；

步骤2：使用线性法直接求磁极子欧拉方程的闭集解，快速估计磁极子参数的初值；

步骤3：使用奇异值截断SDM、TSDM以及迭代TSDM、ITSDM对初始估计进行调整，实现鲁棒且快速的磁极子定位，已知磁极子定位目标函数：

其中x∈R⁵为待优化解，对应于磁极子的位置和仰角，B(x)为磁场模型函数，y表示磁场测量值，针对(2),基于梯度的迭代求解公式为：

其中α为更新步长，J_B为雅可比矩阵，A为缩放因子，SDM通过对解空间采样建立训练集，学习广义梯度

使得：

其中k＝0,1,2,...,T为迭代步数，

表示第i个采样点，X_k＝x_k1^T∈R^5×m；

c为磁传感器总数，在训练阶段，当k＝0时,R₀可以通过求解线性最小二乘问题获得：

其中ΔX＝x₀1^T-X_*,x₀为初始估计解,

(5)的解析解为：

其中

表示Ψ₀的伪逆运算，于是x₁＝x₀-R₀Ψ₀，继而推广到k≥1的情况有：

如此逐步迭代学习一系列的{R_k},直到

收敛，在测试阶段，SDM直接利用训练得到的广义梯度{R_k}逐步更新x，实现目标函数(2)的最小化，更新公式如下：

x_k＝x_k-1-R_k[B(x_k-1)-y] (8)

其中，y表示磁场测量值，y受到加性噪声n的影响时，x_k的更新误差可以表示为

具体的，所述

采取奇异值截断方法计算得到，丢弃较小奇异值所对应的成分，然后沿着磁场强度最大差异的方向学习R_k，Ψ_k的SVD可以表示为：

Ψ_k＝U∑V^T (9)

其中

为正交矩阵，假设样本点数m≥3c，rank(Ψ_k)＝n，n≤3c，则

可以表示为：

其中Λ_n×n＝diag(λ₁,λ₂,...,λ_n)，λ₁≥λ₂≥...≥λ_n是非零奇异值，因此Ψ_k可以表示为：

其中，奇异向量u_i和v_i分别是u和v的第i列，奇异值λ_i是第i个分量的尺度因子，对应于最大奇异值λ的奇异向量u_i表示Ψ_k最大方差的方向；

具体的，所述TSDM的广义梯度可以如下表示：

其中r＜n，并保证

ITSDM将上一次TSDM算出来的最优解作为下一次TSDM的初始值，不断更新采样点。

Images