面向大工作空间磁跟踪的自适应区域选择算法

An Adaptive Region Selection Algorithm for Large-Workspace Magnetic Tracking

摘要

针对大工作空间磁性胶囊位姿定位中， 传感阵列尺度增大导致信号解算中面临远场磁信号信噪比降低、 近场偶极子模型失配以及全阵列解算计算量上升等问题， 提出一种基于区域选择的WPSO-LM位姿解算方法。该方法在全阵列测量数据中采用加权粒子群优化（Weighted Particle Swarm Optimization， WPSO）进行粗定位； 随后， 以粗定位结果为中心构建最优解算环带， 环带内外径分别依据偶极近似误差上界与远场衰减规律确定， 主动规避近场模型失配并筛选出信噪比较高的传感器； 最后， 根据环带内传感器实时信噪比， 自适应选取k个最优传感器构成动态子阵列， 采用Levenberg-Marquardt（LM）算法精准解算实时位姿。在六轴机械臂平台上开展的实验表明， 在45 cm×35 cm×15 cm工作空间内， 该方法平均定位误差为1.49 mm， 姿态误差为0.59°， 更新速率为50.20 Hz。

Abstract

In large-workspace magnetic capsule pose localization， multiple critical challenges in signal inversion are introduced as the scale of the sensor array is increased. These challenges include a reduced signal-to-noise ratio （SNR） of far-field magnetic signals， mismatch of the dipole model in the near field， and a growing computational burden associated with full-array solving. To address these issues， a region-selection-based WPSO-LM pose estimation method is proposed. Firstly， weighted particle swarm optimization （WPSO） is employed to obtain a coarse localization result from the full-array measurement data. An optimal solving annulus is then constructed around the coarse estimate， with its inner and outer radii determined according to the upper bound of the dipole approximation error and the far-field attenuation law， respectively. In this way， near-field model mismatch is avoided， while sensors with relatively high SNR are retained. Subsequently， k optimal sensors within the annulus are adaptively selected according to their real-time SNRs to form a dynamic subarray， and the Levenberg-Marquardt （LM） algorithm is applied for accurate real-time pose estimation. Experiments conducted on a six-axis robotic arm platform demonstrate that， within a workspace of 45 cm×35 cm×15 cm， the proposed method achieves an average position error of 1.49 mm， an orientation error of 0.59°， and an update rate of 50.20 Hz.

Graphical abstract

关键词

磁跟踪系统 / 自适应区域选择 / WPSO-LM优化算法 / 磁传感阵列 / 磁偶极子模型

Key words

magnetic tracking system / adaptive region selection / WPSO-LM optimization algorithm / magnetic sensor array / magnetic dipole model

0 引　言

消化道疾病临床筛查中， 磁控胶囊内镜机器人凭借无创、 舒适、 安全等优势， 已成为传统侵入式内镜的重要替代方案。而实现胶囊机器人的精准运动控制与可靠病灶定位的核心前提在于具备实时、 稳定且高精度的体内位姿感知能力^［1‐3］。在众多外部定位方案中， 基于静磁场的磁跟踪技术无需视线、 不受光学环境干扰， 且易于与磁驱动系统集成， 已成为胶囊机器人定位的重要研究方向^［4‐5］。

现有磁跟踪系统以胶囊内置永磁体为磁源， 体外布设多通道磁传感器阵列， 基于磁偶极子模型建立磁场强度与磁源位姿的非线性映射关系， 通过非线性最小二乘方法反演五维位姿^［6‐7］。其中， Levenberg-Marquardt（LM）算法因收敛速度快、 定位精度高， 被广泛应用于磁信号位姿解算， 但该算法对初始迭代值敏感， 易陷入局部最优^［8］。为缓解初始值依赖问题， 研究者将粒子群优化（Particle Swarm Optimization， PSO）与LM结合， 构建混合定位框架， 提升全局搜索能力与收敛稳定性^［9‐10］。然而， 这类方法多采用固定传感器阵列、 固定数据通道的解算模式， 无法依据磁源实时位置自适应筛选高信息量观测区域， 难以适配大尺寸阵列的动态定位需求^［11］。随着传感阵列尺寸增大， 远场传感器与磁源的距离增加， 而磁场信号会随距离三次方衰减， 远场有效信号减弱， 进而造成观测信号信噪比降低^［12］。在近场区域， 永磁体高阶多极矩分量的影响不可忽略， 采用偶极子模型会引入模型失配误差^［13］。此时， 若直接采用全阵列所有通道数据进行解算， 会不可避免地引入低信噪比观测值^［14］及模型失配严重的观测信号， 进而降低LM算法的收敛精度。

现有研究主要对传感器布局进行离线优化， 分析阵列拓扑与间距对误差分布的影响^{［15‐17］}。但此类离线优化方案无法在线调整， 难以随磁源实时运动动态平衡信号质量、 模型适配性与计算成本。尽管无线定位领域已有基于Cramér-Rao界的动态传感器选择策略， 但面向静磁场胶囊定位， 同时考虑信噪比衰减与模型失配的在线联合优化机制仍待深入研究^［18］。

针对上述局限， 本文基于5×5磁阻传感器阵列平台， 提出一种基于最优解算环带的动态区域解算算法， 并与加权粒子群-LM（Weighted Particle Swarm Optimization-Levenberg-Marquardt， WPSO-LM）框架融合， 以实现大空间内五维位姿的高精度实时跟踪。该方法通过粗定位划定最优解算环带， 自适应筛选出高信噪比、 低模型误差的传感器子集， 在保证定位精度的同时有效降低计算量， 从而实现对磁源精准稳定的实时跟踪。

1 系统工作流程

图 1 为系统工作流程。

首先， 使用WPSO算法在全阵列数据上进行粗定位， 获得磁源的粗略位姿{\mathit{\eta }}_{\mathrm{0}}； 接着， 以{\mathit{\eta }}_{\mathrm{0}}位置为圆心， 在阵列平面构建具有明确物理意义的径向最优解算环带， 环带外径由偶极子远场衰减规律与传感器噪声水平共同确定， 以确保所选传感器具备足够的信噪比， 内径则依据有限尺寸永磁体的多极展开模型所给出的偶极近似误差上界设定， 用于主动剔除近场模型失配严重的数据。在此基础上， 算法根据环带内传感器的实时信噪比， 自适应选取信噪比最高的k个传感器数据， 仅利用这部分随目标运动动态更新的优质观测数据执行LM解算， 实现实时高精度位姿解算。

2 最优解算环带确定

2.1 磁偶极子模型

基于胶囊机器人所载微型永磁体的尺寸约束， 本文采用图 2 所示的磁偶极子模型为定位理论基础， 空间任意一点磁场 B 可表示为

\left\{\begin{array}{l}\mathit{B}=\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}}[\mathrm{3}(\mathit{m}\cdot \mathit{r})\mathit{r}-\mathit{m}],\\ \mathit{m}=m\cdot (n,p,q),\end{array}\right.（1）

式中： μ₀为真空磁导率； μ_r为相对磁导率； r 为磁源到场点的单位矢量； m 为磁铁的磁矩矢量； （n，p，q）为磁铁的单位磁矩矢量； m为磁矩模量。图 2 中， （a，b，c）为场源位置； （x，y，z）为传感器位置坐标； r为磁偶极子中心到空间待求场点的距离。

2.2 最优环带上界确定

由于磁场强度随距离增加而衰减， 所以在远场， 传感器噪声成为测量误差的主要来源。本文将传感器噪声建模为方差σ²的高斯白噪声并引入信噪比来量化观测可靠性，

SNR=\frac{P_{\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}\mathrm{a}\mathrm{l}}}{P_{\mathrm{n}\mathrm{o}\mathrm{i}\mathrm{s}\mathrm{e}}}=\frac{{‖\mathit{B}‖}^{\mathrm{2}}}{{\delta }^{\mathrm{2}}}\mathrm{。}（2）

本文在远场r处使用偶极子模型近似磁源幅值， 有

‖\mathit{B}‖=\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathbf{r}}}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}}‖\mathrm{3}(\mathit{m}\cdot \mathit{r})\mathit{r}-\mathit{m}‖\mathrm{。}（3）

设θ为磁矩矢量 m 与单位位置矢量 r 的夹角， 有

\mathit{m}\cdot \mathit{r}=m\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{。}（4）

将 m 分解到垂直于 r 方向与平行于 r 方向， 有

\begin{array}{c}\mathrm{3}(\mathit{m}\cdot \mathit{r})\mathit{r}-\mathit{m}=\mathrm{3}m\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathit{r}-\\ (m\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathit{r}+m\mathrm{s}\mathrm{i}\mathrm{n}\theta {\mathit{r}}_{\perp })=\mathrm{2}m\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathit{r}-m\mathrm{s}\mathrm{i}\mathrm{n}\theta {\mathit{r}}_{\perp }\mathrm{。}\end{array}（5）

故

\begin{array}{c}{‖\mathit{B}‖}^{\mathrm{2}}={\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}}\right)}^{\mathrm{2}}{‖\mathrm{2}m\mathrm{c}\mathrm{o}\mathrm{s}\theta \mathit{r}-m\mathrm{s}\mathrm{i}\mathrm{n}\theta {\mathit{r}}_{\perp }‖}^{\mathrm{2}}=\\ {\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}}\right)}^{\mathrm{2}}(\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\theta )\mathrm{。}\end{array}（6）

则信噪比为

SNR=\frac{{‖\mathit{B}‖}^{\mathrm{2}}}{{\sigma }^{\mathrm{2}}}={\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}\sigma }\right)}^{\mathrm{2}}(\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\theta )\mathrm{。}（7）

为确保检测信号的可靠性， 本文给定信噪比的最低阈值SNR_min， 故r_max为

r_{\mathrm{m}\mathrm{a}\mathrm{x}}\le {\left({\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}\sigma \sqrt[]{SN{R}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)}^{\mathrm{2}}(\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\theta )\right)}^{\frac{\mathrm{1}}{\mathrm{3}}}\mathrm{。}（8）

上述分析表明， 最优环带的上界r_max受到θ的影响。为提升磁源定位算法的效率， 对r_max进行如下简化，

\begin{array}{c}{r}_{\mathrm{m}\mathrm{a}\mathrm{x}}\le {\left({\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}\sigma \sqrt[]{SN{R}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)}^{\mathrm{2}}(\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\theta )\right)}^{\frac{\mathrm{1}}{\mathrm{3}}}\le \\ {\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}\sigma \sqrt[]{SN{R}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)}^{\frac{\mathrm{2}}{\mathrm{3}}}\mathrm{。}\end{array}（9）

故最优环带上界确定为

r_{\mathrm{m}\mathrm{a}\mathrm{x}}={\left(\frac{{\mu }_{\mathrm{0}}{\mu }_{\mathrm{r}}m}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}\sigma \sqrt[]{SN{R}_{\mathrm{m}\mathrm{i}\mathrm{n}}}}\right)}^{\frac{\mathrm{2}}{\mathrm{3}}}\mathrm{。}（10）

2.3 最优环带下界确定

在近场条件下， 随着磁源与传感器距离减小， 有限尺寸磁体的实际磁场分布与理想点偶极子模型的偏差增大。因此， 需要给出一个距离下界r_min保证模型有效。

本文将有限尺寸磁体的真实磁感应强度记为 B_real， 磁偶极子模型的预测记为 B_dip。定义相对模型误差ε为

\epsilon =\frac{‖{\mathit{B}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l}}-{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}‖}{‖{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}‖}\mathrm{。}（11）

对于轴向均匀磁化的永磁体， 空间任意一点磁场{\mathit{B}}_{\mathbf{t}\mathbf{o}\mathbf{t}\mathbf{a}\mathbf{l}}可用磁标势描述为

{\mathit{B}}_{\mathbf{t}\mathbf{o}\mathbf{t}\mathbf{a}\mathbf{l}}=-{\mu }_{\mathrm{0}}\nabla \Phi ,（12）

式中： Ф为磁标量势。在球坐标系（r， ϑ， ψ）中， 磁标势可展开为多极级数

{\Phi }_K(r,\vartheta )=\frac{\mathrm{1}}{\mathrm{4}\pi }{\displaystyle \sum _{K=\mathrm{0}}^{\mathrm{\infty }}}\frac{Q_K}{r^{K+\mathrm{1}}}{P}_K(\mathrm{c}\mathrm{o}\mathrm{s}\vartheta ),（13）

P_K(x)=\frac{\mathrm{1}}{{\mathrm{2}}^{K}K!}\frac{{\mathrm{d}}^K}{\mathrm{d}{x}^K}[(x^{\mathrm{2}}{-\mathrm{1})}^K],（14）

式中： Q_K 为第K阶多极矩； P_K 为第K阶勒让德多项式； r为场点到磁体中心的距离； ϑ为球坐标极角。

磁场多极展开的首项为偶极子近似， 第一项修正是八极子项。本文将偶极子模型预测误差主要归因于八极子项贡献。图 3 所示为轴向磁化圆柱体， 其半径为R， 长度为L， 磁化强度为M。

偶极矩Q₁为

Q_{\mathrm{1}}=\mathrm{\pi }ML{R}^{\mathrm{2}}\mathrm{。}（15）

八极矩Q₃为

Q_{\mathrm{3}}=\frac{\mathrm{\pi }ML{R}^{\mathrm{2}}}{\mathrm{4}}(L^{\mathrm{2}}-\mathrm{3}{R}^{\mathrm{2}})\mathrm{。}（16）

偶极子项标势为

{\Phi }_{\mathrm{1}}(r,\vartheta )=\frac{Q_{\mathrm{1}}}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{2}}}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta \mathrm{。}（17）

八极子项标势为

{\Phi }_{\mathrm{3}}(r,\vartheta )=\frac{Q_{\mathrm{3}}}{\mathrm{8}\mathrm{\pi }{r}^{\mathrm{4}}}(\mathrm{5}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{3}}\vartheta -\mathrm{3}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta )\mathrm{。}（18）

在球坐标系（r，ϑ，ψ）中， 由于轴对称性，所有的场量与方位角ψ无关， 有

\nabla \Phi =\left(\frac{\partial \Phi }{\partial r},\frac{\mathrm{1}}{r}\frac{\partial \Phi }{\partial \vartheta },\mathrm{0}\right)\mathrm{。}（19）

故偶极子项磁场径向分量为

{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}(r)=-u_{\mathrm{0}}\frac{\partial {\Phi }_{\mathrm{1}}}{\partial r}=\frac{u_{\mathrm{0}}{Q}_{\mathrm{1}}}{\mathrm{2}\mathrm{\pi }{r}^{\mathrm{3}}}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta \mathrm{。}（20）

偶极子项磁场极角分量为

{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}(\vartheta )=-u_{\mathrm{0}}\frac{\mathrm{1}}{r}\frac{\partial {\Phi }_{\mathrm{1}}}{\partial \vartheta }=\frac{u_{\mathrm{0}}{Q}_{\mathrm{1}}}{\mathrm{4}\pi r^{\mathrm{3}}}\mathrm{s}\mathrm{i}\mathrm{n}\vartheta \mathrm{。}（21）

偶极子项磁场为

\begin{array}{c}‖{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}‖=\sqrt[]{{‖{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}(r)‖}^{\mathrm{2}}+{‖{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}(\vartheta )‖}^{\mathrm{2}}}=\\ \frac{u_{\mathrm{0}}{Q}_{\mathrm{1}}}{\mathrm{4}\mathrm{\pi }{r}^{\mathrm{3}}}\sqrt[]{\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\vartheta }\mathrm{。}\end{array}（22）

八极子项磁场径向分量为

{\mathit{B}}_{\mathrm{3}}(r)=-u_{\mathrm{0}}\frac{\partial {\Phi }_{\mathrm{3}}}{\partial r}=\frac{u_{\mathrm{0}}{Q}_{\mathrm{3}}}{\mathrm{2}\mathrm{\pi }{r}^{\mathrm{5}}}(\mathrm{5}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{3}}\vartheta -\mathrm{3}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta )\mathrm{。}（23）

八极子项磁场极角分量为

{\mathit{B}}_{\mathrm{3}}(\vartheta )=-u_{\mathrm{0}}\frac{\mathrm{1}}{r}\frac{\partial {\Phi }_{\mathrm{3}}}{\partial \vartheta }=\frac{\mathrm{3}{u}_{\mathrm{0}}{Q}_{\mathrm{3}}\mathrm{s}\mathrm{i}\mathrm{n}\vartheta }{\mathrm{8}\mathrm{\pi }{r}^{\mathrm{5}}}(\mathrm{5}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\vartheta -\mathrm{1})\mathrm{。}（24）

八极子项磁场为

‖{\mathit{B}}_{\mathrm{3}}‖=\sqrt[]{{‖{\mathit{B}}_{\mathrm{3}}(r)‖}^{\mathrm{2}}+{‖{\mathit{B}}_{\mathrm{3}}(\vartheta )‖}^{\mathrm{2}}}=\frac{{\mu }_{\mathrm{0}}{Q}_{\mathrm{3}}}{\mathrm{\pi }{r}^{\mathrm{5}}}f(\vartheta ),（25）

f(\vartheta )=\frac{\sqrt[]{\mathrm{175}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{6}}\vartheta -\mathrm{165}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{4}}\vartheta +\mathrm{45}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\vartheta +\mathrm{9}}}{\mathrm{8}}\mathrm{。}（26）

相对模型误差ε为

\epsilon =\frac{‖{\mathit{B}}_{\mathrm{3}}‖}{‖{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}‖}=\left|\frac{\mathrm{4}{Q}_{\mathrm{3}}}{Q_{\mathrm{1}}{r}^{\mathrm{2}}}\right|\frac{f(\vartheta )}{\sqrt[]{\mathrm{1}+\mathrm{3}\mathrm{c}\mathrm{o}{\mathrm{s}}^{\mathrm{2}}\vartheta }}\mathrm{。}（27）

设相对模型误差的最大阈值为ε_max， 则选取的最优环带的下界r_min为

r_{\mathrm{m}\mathrm{i}\mathrm{n}}\ge \sqrt[]{\frac{\left|L^{\mathrm{2}}-\mathrm{3}{R}^{\mathrm{2}}\right|}{\mathrm{2}{\epsilon }_{\mathrm{m}\mathrm{a}\mathrm{x}}}}\mathrm{。}（28）

最优环带下界确定为

r_{\mathrm{m}\mathrm{i}\mathrm{n}}=\sqrt[]{\frac{\left|L^{\mathrm{2}}-\mathrm{3}{R}^{\mathrm{2}}\right|}{\mathrm{2}{\epsilon }_{\mathrm{m}\mathrm{a}\mathrm{x}}}}\mathrm{。}

3 磁跟踪算法

磁场位姿估计属于非线性最小二乘问题， 目标为最小化实测磁场与模型预测值之间的残差。LM算法通过阻尼因子自适应融合高斯-牛顿法与梯度下降法， 兼具快速局部收敛性与病态雅可比鲁棒性。相较于PSO等随机优化方法， LM算法每步仅需求解低维线性系统， 在磁位姿解算中收敛更快、 精度更高， 因此， 本文将其作为核心精解算法。针对LM对初始值敏感的问题， 本文采用WPSO实现粗定位， 为LM迭代求解提供可靠初始估计。

3.1 WPSO初始位置估计

本文使用WPSO方法估计初始位姿 η₀=（a₀，b₀，c₀，n₀，p₀，q₀）， 传感阵列中各芯片实测磁场幅值可表示为

{\omega }_i=‖{\mathit{B}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}‖=\sqrt[]{B{X}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}^{\mathrm{2}}+B{Y}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}^{\mathrm{2}}+B{Z}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}^{\mathrm{2}}}\mathrm{。}（29）

磁场幅值越大的传感芯片越接近磁源， 其权重越高。加权质心位置计算为

{\mathit{x}}_{\mathrm{0}}={\displaystyle \sum _{i=\mathrm{1}}^N}{\omega }_i(x_i,y_i,z_i)/{\displaystyle \sum _{i=\mathrm{1}}^N}{\omega }_i,（30）

式中： x₀为加权质心位置； （x_i，y_i，z_i ）为第i个传感器位置。基于二维传感阵列平面检测的磁场信息， 加权质心法仅能估计胶囊（x，y）坐标， 无法确定Z轴坐标。通过对加权质心法的求解结果设定约束半径δ， 可得X和Y的解空间为

\left\{\begin{array}{l}a\in [{\mathit{x}}_{\mathrm{0}}(\mathrm{1})-\delta ,{\mathit{x}}_{\mathrm{0}}(\mathrm{1})+\delta ],\\ b\in [{\mathit{x}}_{\mathrm{0}}(\mathrm{2})-\delta ,{\mathit{x}}_{\mathrm{0}}(\mathrm{2})+\delta ]\mathrm{。}\end{array}\right.（31）

本文取\delta为d/2， d为相邻传感器的间距。针对解空间约束后的位姿向量 η₀， 采用PSO算法进行优化。算法在6维空间中初始化一个包含N个粒子的种群。粒子的速度与位置更新函数以及评估函数为

\begin{array}{c}{\mathit{\nu }}_{\mathit{i}}^{\mathit{t}+\mathrm{1}}=\omega \cdot {\mathit{\nu }}_{\mathit{i}}^{\mathit{t}}+c_{\alpha }\cdot ran{d}_{\mathrm{1}}\cdot ({\mathit{P}}_{\mathbf{b}\mathbf{e}\mathbf{s}\mathbf{t},\mathit{i}}^{\mathit{t}}-{\mathit{X}}_{\mathit{i}}^{\mathit{t}})\dots \dots +\\ c_{\beta }\cdot ran{d}_{\mathrm{2}}\cdot ({\mathit{g}}_{\mathbf{b}\mathbf{e}\mathbf{s}\mathbf{t}}^{\mathit{t}}-{\mathit{X}}_{\mathit{i}}^{\mathit{t}}),\end{array}（32）

\left\{\begin{array}{l}{\mathit{X}}_{\mathit{i}}^{\mathit{t}+\mathrm{1}}={\mathit{X}}_{\mathit{i}}^{\mathit{t}}+{\mathit{\nu }}_{\mathit{i}}^{\mathit{t}+\mathrm{1}},\\ {\mathit{X}}_{\mathit{i}}^{\mathit{t}+\mathrm{1}}=clip({\mathit{X}}_{\mathit{i}}^{\mathit{t}+\mathrm{1}},X_{\mathrm{m}\mathrm{i}\mathrm{n}},X_{\mathrm{m}\mathrm{a}\mathrm{x}}),\end{array}\right.（33）

\left\{\begin{array}{l}\mathit{F}({\mathit{X}}_i)={\displaystyle \sum _{i=\mathrm{1}}^S}{‖{\mathit{B}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}-{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p},i}+{\mathit{B}}_{\mathrm{e},i}‖}^{\mathrm{2}},\\ s.t{.}_{}{}_{}{}_{}{}_{}{}_{}{}_{}{}_{}{n}^{\mathrm{2}}+p^{\mathrm{2}}+q^{\mathrm{2}}=\mathrm{1},\end{array}\right.（34）

式中： {\mathit{X}}_i为每个粒子位置且{\mathit{X}}_i=（a_i，b_i，c_i，n_i，p_i，q_i）； v_i为粒子运动速度； {\mathit{B}}_{\mathrm{e}}为预先检测的环境磁场； c_{\alpha }个体学习因子； c_{\beta }为群体学习因子； \omega为惯性权重； rand1， rand2为0~1之间的随机数； \mathit{F}({\mathit{X}}_i)为目标函数； {\mathit{P}}_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t},i}为个体最优位置； g_{\mathrm{b}\mathrm{e}\mathrm{s}\mathrm{t}}为群体最优位置。

首次迭代后记录新的粒子种群的个体最优 P_best，i 和群体最优g_best， 当迭代满足收敛条件， 输出g_best对应的粒子位置（a₀，b₀，c₀，n₀，p₀，q₀）记作 η₀， 作为基于区域选择的LM算法的初始位姿。

3.2 基于区域选择的WPSO-LM算法

区域选择方法的目的是从全局传感器网络中选出一组既能保证高信噪比， 又能满足磁偶极子模型的观测子集。

胶囊机器人初始位姿 η₀已由WPSO算法给出， 对于全场S个磁传感器， 其空间位置为（x_i，y_i，z_i ） （i=1， 2…S）， 磁源到传感器的预估距离r_i 为

r_i=\sqrt[]{(a_{\mathrm{0}}-x_i{)}^{\mathrm{2}}+(b_{\mathrm{0}}-y_i{)}^{\mathrm{2}}+(c_{\mathrm{0}}-z_i{)}^{\mathrm{2}}}\mathrm{。}（35）

如图 4 所示， 最优环带（r_min， r_max）取位于环带内的所有磁传感数据， 依据式（7）选取SNR最高的前k个传感数据构成最终参与解算的最优传感器子集K_s。随后， 以区域选择策略得到的传感器子集K_s 和粗估位姿 η₀作为输入， 采用LM算法进行高精度优化。对于LM算法， 定义残差函数f为测量值与模型预测值之差， 则

f(\mathit{\eta })={\mathit{B}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l}}-{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p}}(\mathit{\eta })+{\mathit{B}}_{\mathrm{e}}\mathrm{。}（36）

定义目标函数 E 为

\left\{\begin{array}{l}\mathit{E}={\displaystyle \sum _{i=\mathrm{1}}^k}{‖{\mathit{B}}_{\mathrm{r}\mathrm{e}\mathrm{a}\mathrm{l},i}-{\mathit{B}}_{\mathrm{d}\mathrm{i}\mathrm{p},i}+{\mathit{B}}_{\mathrm{e},i}‖}^{\mathrm{2}},\\ s.t{.}_{}{}_{}{}_{}{}_{}{n}^{\mathrm{2}}+p^{\mathrm{2}}+q^{\mathrm{2}}=\mathrm{1}\mathrm{。}\end{array}\right.（37）

LM算法通过式（40）迭代更新参数估计。

{\mathit{\eta }}_{t+\mathrm{1}}={\mathit{\eta }}_t-(\mathit{J}({\mathit{\eta }}_t){\mathit{J}}^{\mathrm{T}}({\mathit{\eta }}_t{)+\mu \mathit{I})}^{-\mathrm{1}}\mathit{J}({\mathit{\eta }}_t)f({\mathit{\eta }}_t),（38）

式中： η_t 为第t次迭代结果； J （ η_t ）为当前迭代步骤的雅可比矩阵值； μ为阻尼因子； I 为单位矩阵； f（ η_t ）为当前迭代过程中残差矩阵的数值计算结果。当迭代满足收敛条件时， 输出基于当前磁场数据解算的最优位姿 η_t+1作为当前胶囊机器人的位置姿态（a，b，c，n，p，q）。在连续解算中， 将前一次的解算结果（a，b，c，n，p，q）作为下一次的LM解算的粗估位姿 η₀。

4 实验验证

4.1 磁跟踪系统

磁跟踪系统（图 5）主要由机械臂（DOBOT CR10A）、 磁性传感阵列、 多路复用模块、 单片机及上位机构成。

磁性传感阵列基于一块尺寸为45 cm×35 cm的马克板构建， 其上以5×5矩阵形式均匀布置了25个MAG3110磁阻传感器。数据选择模块采用CD4067BE多路复用芯片并搭配必要的外围电路实现。阵列中， 25个传感器共享VCC、 GND和SCL线路， 并通过数据选择模块连接至单片机（型号： STM32F103ZET6）。各传感器的SDA引脚则分别接入多路复用器的25个独立通道。单片机通过配置多路复用器的A0-A5地址线以选通相应传感器， 其输出的磁场信号经由单片机的I/O口采集后上传至上位机进行后续处理。上位机用于执行WPSO-LM算法， 实时显示胶囊机器人的位置和姿态。六轴机械臂（DOBOT CR10A）用于携带胶囊机器人执行预设轨迹运动， 进而与实际解算结果进行对比， 以验证系统定位和定向精度。

4.2 对比试验

为验证WPSO-LM算法的性能优势， 本节将所提出的WPSO-LM混合优化方法与WPSO算法、 PSO算法进行对比分析。实验采用COMSOL仿真软件构建了磁源连续运动过程中在传感器阵列位置感生的磁场数据集。该数据集共包含500组样本， 每组样本记录了25个磁传感器位置处的磁场信息。所有数据均在硬件平台（Intel Core i9-14900HX处理器、 32 GB内存、 Windows 11操作系统）上利用MATLAB R2023b进行解算与分析。WPSO-LM参数见表 1， WPSO算法与PSO算法的最大迭代次数为300， 粒子数为100， 惯性权重取0.7， 个体与群体学习因子均取1.5， 函数容差为1×10^-9。实验结果见表 2。

由表 2 可知， 所提出的WPSO-LM算法在定位精度与实时性方面均优于WPSO算法与PSO算法。

4.3 基于区域选择的WPSO-LM算法轨迹精度实验

为评估动态区域选择策略对磁源定位性能的影响， 本研究采用所提出的基于区域选择的WPSO-LM算法开展定位实验， 并对定位精度与计算效率进行验证。算法关键参数列于表 1， 定位测试对象为图 6 所示的轴向充磁圆柱永磁体。

实验中， 机械臂携带磁源沿预设轨迹运动。本文以机械臂末端在实验坐标系下的标准位姿作为磁源位姿的真值， 并将其与算法输出的估计位姿进行逐点比较， 从而计算定位误差。实验结果如图 7 所示。

根据实验结果， 在45 cm×35 cm×15 cm的工作平面内， 算法共拟合得到6 024个位姿点， 与实际轨迹对比可知， X轴误差为（0.93±0.67） mm， Y轴误差为（0.65±0.48） mm， Z轴误差为（0.63±0.56） mm， 三轴误差为（1.49±0.68） mm。在实时性能方面， 位姿解算及系统GUI界面显示的平均更新频率达到50.20 Hz。实验结果表明， 本文所提出的跟踪与定位方法在复杂三维轨迹条件下具有良好的精度和稳定性。

4.4 基于区域选择的WPSO-LM算法定向实验

本文采用与轨迹精度实验相同的磁跟踪系统架构进行定向实验。在实验过程中， 机械臂在45 cm×35 cm×15 cm的工作平面内携带磁源运动至随机位置， 并在每个位置处随机调整姿态， 取1 000个姿态点位进行姿态解算。实验结果见图 8。

依据实验结果可知，俯仰角误差均值为0.46°， 标准差为0.54°， 航向角误差均值为0.21°， 标准差为0.30°， 定向误差均值为0.59°， 标准差为0.62°。

4.5 与现有研究对比

为验证所提方法的综合性能， 本文从跟踪范围、 检测精度与单帧执行时间等方面与现有研究进行对比， 结果见表 3。

由表 3 可知， 与LM、 PSO-LM和线性LM方法相比， 本文方法在定位精度和定向精度方面均明显提升， 且计算时间更短。虽然LM-PSO方法具有更高的定位和定向精度， 但其单帧执行时间高达830 s， 难以满足实时跟踪需求。相比之下， 本文方法在保证较高跟踪精度的同时达成了实时计算的需求， 为胶囊机器人大区域实时磁定位提供了一种方法。

5 结　论

本文提出了一种基于区域选择的WPSO-LM位姿解算算法， 45 cm×35 cm×15 cm的工作空间内的实验可知， 该方法平均定位误差为1.49 mm， 姿态误差为0.59°， 连续运行时上位机界面平均刷新频率可达50.2 Hz， 具备良好的实时性。与其他方法相比， 本文方法在精度和解算效率上有更好的平衡。结果表明所提出的区域选择策略在保证解算精度的前提下有效压缩了雅可比矩阵规模， 降低了单次迭代的计算量， 为大阵列磁跟踪系统的工程实现提供了解决思路。

参考文献

[1] Than T， Alici G， Zhou H， et al. A review of localization systems for robotic endoscopic capsules［J］. IEEE Transactions on Biomedical Engineering， 2012， 59（9）： 2387-2399.

[2] Bianchi F， Masaracchia A， Barjuei E S， et al. Localization strategies for robotic endoscopic capsules： a review［J］. Expert Review of Medical Devices， 2019， 16（5）： 381-403.

[3] Hanscom M， Cave D R. Endoscopic capsule robot-based diagnosis， navigation and localization in the gastrointestinal tract［J］. Frontiers in Robotics and AI， 2022， 9： 896028.

[4] Ali M A， Tom N， Alsunaydih F N， et al. Recent advancements in localization technologies for wireless capsule endoscopy： a technical review［J］. Sensors， 2025， 25（1）： 253.

[5] Zeising S， Anzai D， Thalmayer A， et al. Innovative differential magnetic localization method for capsule endoscopy to prevent interference caused by the geomagnetic field［J］. Advances in Radio Science， 2021， 19： 207-213.

[6] Fu Q， Fan C， Wang X， et al. A compensation method for magnetic localization on capsule robot in medical application［J］. Sensors Journal， 2021， 21（23）： 26690-26698.

[7] Hou Z W， Chen C X， Ma T H， et al. Design of driving wheel speed test device based on magnetoelectric sensor［J］. Journal of Measurement Science and Instrumentation， 2020， 11（2）： 186-192.

[8] Suveren M， Akay R， Yildirim M Y， et al. Application of hybrid metaheuristic with Levenberg-Marquardt algorithm for 6-dimensional magnetic localization［J］. Evolving Systems， 2022， 13（6）： 849-867.

[9] Xuan Z， Shanshan S， Jiansheng X. A positioning system of capsule endoscope based on the combination of particle swarm optimization and L-M algorithm［C］//2019 3rd International Conference on Electronic Information Technology and Computer Engineering （EITCE）， 2019： 1263-1267.

[10] Yang W， Hu C， Meng M， et al. A six-dimensional magnetic localization algorithm for a rectangular magnet objective based on a particle swarm optimizer［J］. IEEE Transactions on Magnetics， 2009， 45（8）： 3092-3099.

[11] Xu Y， Li K， Zhao Z， et al. Adaptive simultaneous magnetic actuation and localization for wce in a tubular environment［J］. IEEE Transactions on Robotics， 2022， 38（5）： 2812-2826.

[12] Géron G， Prelle C， Al Hajjar H， et al. Characterization of a magnetic localization method based on Hall effect sensor array for microrobot position tracking［J］. Journal of Micro and Bio Robotics， 2022， 18（1）： 1-13.

[13] Xu W， Wang W， Cao F， et al. A sensor localization and orientation method for OPM-MEG based on rigid coil structures and magnetic dipole fitting models［J］. Bioengineering， 2025， 12（11）： 1198.

[14] Su S， Yang W， Dai H， et al. Investigation of the relationship between tracking accuracy and tracking distance of a novel magnetic tracking system［J］. IEEE Sensors Journal， 2017， 17（15）： 4928-4937.

[15] Song S， Qiu X， Wang J， et al. Design and optimization strategy of sensor array layout for magnetic localization system［J］. IEEE Sensors Journal， 2017， 17（6）： 1849-1857.

[16] Chen S， Zhu M， Zhang Q， et al. Design and optimization of sensor array for magnetic gradient tensor system［J］. Sensor Review， 2020， 40（1）： 121-129.

[17] Kim M C， Kim E S， Park J O， et al. Robotic localization based on planar cable robot and hall sensor array applied to magnetic capsule endoscope［J］. Sensors， 2020， 20（20）： 5728.

[18] Oh M S， Hosseinalipour S， Kim T， et al. dynamic and robust sensor selection strategies for wireless positioning with TOA/RSS measurement［J］. IEEE Transactions on Vehicular Technology， 2023， 72（11）： 14656-14672.

[19] Song S， Li B， Qiao W， et al. 6-D Magnetic localization and orientation method for an annular magnet based on a closed-form analytical model［J］. IEEE Transactions on Magnetics， 2014， 50（9）： 5000411.

[20] Hu C， Li M， Song S， et al. A cubic 3-axis magnetic sensor array for wirelessly tracking magnet position and orientation［J］. IEEE Sensors Journal， 2010， 10（5）： 903-913.

基金资助

国家自然科学基金面上资助项目(52275038)

山西省优秀青年培育资助项目(202203021224007)

中国博士后科学基金第18批特别资助项目(2025T180991)

中国博士后科学基金第76批面上资助项目(2024M763026)