高背景磁场下的差分磁定位算法及应用

将式(7)表示成如下形式：
fku (α ) = Bku
(8)
式中：k 为磁传感器组数，k=1, 2, …, N；u=x、y、z。
引入矩阵 Bu、fu 和对角阵 Λ ：
{( ) ( ) ( ) } Bu =
B1u
T
,
B2u
T ,",
B Nu
T
T
(9)
{( ) ( ) ( ) } f u =
f
u 1
(α
3rixriy 3riy2 − ri2
3riy riz
3rixriz 3riy riz
⎤
⎥ ⎥
⋅
r −5 i
−
3riz2 − ri2 ⎥⎦
(6)
⎢⎡(3r(2i +1) x
−
r2 ( i+1)
)
⎢ 3r r (i+1) x (i+1) y
⎢ ⎢⎣
3r r (i +1) x (i+1) z
3r r (i+1) x (i+1) y
摘 要：为了有效去除高背景磁场对永磁体定位的磁干扰，本文提出了一种高背景磁场下的差分磁定位算法。在背景磁场大 小和方向均相等的两位置点处布置一组磁传感器，每组传感器均采用差分放大电路，共八组通路。在高背景磁场下，运用差 分磁定位算法反演永磁体的位置和姿态。并将该算法应用于外磁场导向的仿生微型机器人，设计了定位硬件系统。通过实验 验证了此差分磁定位算法可以较好地反演该仿生微型机器人位置和姿态。 关键词：差分磁定位算法；磁偶极子；圆柱永磁体；仿生微型机器人 中图分类号：TP202+.2 文献标识码：A 国家标准学科分类代码：140.3530
永磁体的位置和姿态分别由 R 和 m 表示。磁矩 m 中
不包含永磁体自旋信息，因此磁定位不能确定永磁体的
自旋状态。磁矩 m 可以采用方位角(θ, φ)来描述（如图 1
所示），为：
⎧⎪mx ⎨my
= =
m sin θ m sin θ
cosϕ sinϕ
(3)
⎪⎩mz = mcosθ
图 1 永磁体位置和方向定义 Fig.1 Definition of localization and orientation
)
T
,
f
u 2
(α
)
T ,",
f
u N
(α
)
T
T
(10)
Λ = diag(a1,a2,", a3N )
(11)
式中：al=0 或 1，l=1,2,…,3N。实际测量中，并非测量测
试组的每个方向，而是测量其某一个或几个方向。对角
第 30 卷 第 11 期 2009 年 11 月
仪器仪表学报
Chinese Journal of Scientific Instrument
Vol.30 No. 11 Nov. 2009
高背景磁场下的差分磁定位算法及应用*
王 喆1,2，宋 涛1，王金光1，2，王 铮1，杨岑玉1,2
（1 中国科学院电工研究所 北京 100190；2 中国科学院研究生院 北京 100049）
Differential magnetic localization algorithm in high background magnetic field and its application
Wang Zhe1,2, Song Tao1, Wang jinguang1,2, Wang Zheng1, Yang Cenyu1,2 (1 Institute of Electrical Engineering, Chinese Academy of Sciences, Beijing 100080, China；
R(x, y, z)和姿态 m(θ, φ)的 5 个未知量为：α=(x, y, z, θ, φ)，
则式(6)可以表示成如下形式：
⎡ ⎢ ⎢
Bkx Bky
⎤ ⎥ ⎥
=
⎡ ⎢ ⎢
f kx fky
(α (α
)⎤ ⎥ )⎥
(7)
⎢ ⎣
Bkz
⎥ ⎦
⎢ ⎣
f kz
(α
)
⎥ ⎦
式中：k 为磁传感器组数，k=1, 2,… , N；i=2k–1。
2 Graduate University of Chinese Academy of Sciences, Beijing 100049, China)
Abstract：This paper reports a differential magnetic localization algorithm in high background magnetic field that interferes localization of permanent magnet. In order to remove the interference, two magnetic sensors are placed in two points where the background magnetic fields are the same. There are eight channels and every channel uses a differential circuit. Then the location and orientation of the permanent magnet are inversed by differential magnetic localization algorithm, which is applied to a bionic micro-machine guided by external field. The hardware system for localization is also designed. Experiments show that the location and orientation of the bionic micro-machine are inversed well by the differential magnetic localization algorithm. Key words：differential magnetic localization algorithm; magnetic dipole; cylindrical permanent magnet; bionic micro-machine
现有的磁定位方法均未涉及高背景磁场下磁定位的 解决方案。针对高背景磁场下的磁定位方法，本文提出了 一种基于磁传感技术的差分磁定位算法。并对我们提出的 主动螺旋推进结合外磁场导向的仿生微型机器人设计了 定位系统[5]，通过实验反演了仿生微型机器人位置和姿态。
2 差分磁定位算法
2.1 圆柱永磁体数学模型 当前，磁偶极子模型被广泛应用于目标物体的定位及
由于人体组织是非磁性的[2]，可以在血管机器人和消化 道机器人内部安装一永磁体，通过检测人体外部的永磁体 空间磁场参数变化来确定永磁体的空间位置，进而实现机
收稿日期：2008-09 Received Date：2008-09 *基金项目：国家 863 计划（2006AA04Z249）资助项目
器人的实时定位[3-4]。由于磁定位方法具有造价低，定位精 度高，使该定位方法具有非常好的研究价值和应用前景。
第 30 卷
式中：ri=[(xi´–x)2+(yi´–y)2+(zi´–z)2]1/2；k 为磁传感器组数， k=1, 2, …, N；i=2k–1。
由式(3)，磁矩的绝对值 m 是一个定值，一旦确定，
就不随时间和空间改变。这样可以将待定的参数减为 5
个：R(x, y, z)和 m(θ, φ)。定义式(6)中的涉及永磁体位置
3( x ' 3( y'
− −
x)( z ' y )( z '
− −
z)⎤ z)⎥⎥
⋅
⎡ mx
⎢ ⎢
my
⎤ ⎥ ⎥
⎢⎣ 3(x' − x)(z' − z) 3( y' − y)(z' − z) 3(z' − z)2 − r2 ⎥⎦ ⎢⎣mz ⎥⎦
式中：r=[(x´–x)2+(y´–y)2+(z´–z)2]1/2。
⎪⎪⎧rjx ⎨rjy
= =
x'j
y
' j
−x −y
(5)
⎪ ⎪⎩ rjz
=
z
' j
−
z
式中：j=1, 2, … , 2N。由上述推导，永磁体等效偶极子
模型的空间等背景磁场的差分定位方程组，如式(6)所示。
⎡ ⎢wenku.baidu.com
Bkx
⎤ ⎥
⎢Bky ⎥
⎢ ⎣
Bkz
⎥ ⎦
=
μ0 4π
⎧⎪⎡⎢(3rix2 − ri2 ) ⎨⎢ 3rixriy ⎪⎩⎢⎣ 3rixriz
图 2 基于磁传感器的差分方式示意图 Fig.2 Chart of differential model based on magnetic sensors
在永磁体磁场和背景磁场共同作用下，两传感器 i 和
i+1 检测的磁场强度分别为 Bi 和 Bi+1。其中，背景磁场分 别为 Bi´和 Bi+1´，且 Bi´= Bi+1´。采用差分电路，使得两传 感器检测到的高背景磁场强度 Bi´和 Bi+1´在差分相减中被 剔除出去，而与永磁体相关的磁场信息保留了下来。
1引 言
为了保证介入式微创伤诊疗的血管机器人和消化道 机器人在诊断和治疗过程的有效性，对其体内的空间位置 进行实时定位就显得格外重要。目前常见的定位方法有[1]： X 射线透视定位、三维实时超声成像定位、核医学影像技 术定位、荧光造影定位等。这些定位方法都需要大型设备， 以及操作复杂、费用高、对人体有伤害等问题。
图 2 中，磁传感器 i 和 i+1 差分后得到三个方向的磁
场强度为：
⎡ ⎢ ⎢
Bkx Bky
⎤ ⎥ ⎥
=
⎡ ⎢
Bix
⎢ Biy
⎤ ⎥ ⎥
−
⎡ ⎢
B(i
+1)x
⎢ B(i+1)y
⎤ ⎥ ⎥
(4)
⎢ ⎣
Bkz
⎥ ⎦
⎢⎣Biz ⎥⎦
⎢ ⎣
B(i
+1)z
⎥ ⎦
式中：Bkx，Bky 和 Bkz 分别为第 k 组传感器差分相减后 X、 Y、Z 三个方向的磁场强度。
⎡3(m ⋅ r)r ⎢⎣ r5
−
m r3
⎤ ⎥⎦
(1)
式中：B(Bx, By, Bz)为检测点的磁感应强度矢量；µ0 为介 质磁导率；m(mx, my, mz)为磁偶极子的磁矩矢量；r(rx, ry, rz) 为磁偶极子到检测点的矢径；r 为矢量 r 的模。由图 1，
r´(x´,y´,z´)为磁传感器的中心矢径，r´一旦确定即为已知；
3r(2i+1) y
−
r2
(i +1)
3r r (i+1) y (i+1) z
} 3r r (i+1)x (i+1)z
3r r (i+1) y (i+1) z
3r − r 2
2
(i +1) z
( i+1)
⎤
⎥ ⎥
⋅r
−5
⎥ (i+1)
⎥⎦
⎡mx ⎤
⋅
⎢⎢my
⎥ ⎥
⎢⎣mz ⎥⎦
2386
仪器仪表学报
of the permanent magnet
2.2 差分磁定位算法 当背景磁场强度较之磁定位所用的永磁体的磁场强度
要高许多时，现有的磁定位方法在使用时会遇到弱信号被
强信号湮没的问题，从而影响定位的准确性。针对高背景 磁场下的磁定位问题，提出了一种差分磁定位算法。
在高背景磁场下，为了去除传感器中强的背景磁场 信号，保留弱的永磁体磁场信号，选择在背景磁场的大 小和方向均相等的两位置点处布置一组磁传感器，共 N 组，2N 个传感器。其中第 k(k=1, 2, …, N)组的传感器 i 和 i+1(i=2k–1)的差分方式示意图如图 2 所示。
图 2 中 R(x, y, z)为永磁体中心点的矢径；ri´(xi´, yi´, zi´) 和 ri+1´(xi+1´, yi+1´, zi+1´)分别为传感器 i 和传感器 i+1 的中 心矢径，两传感器的位置点一旦确定，ri´和 r i+1´即为已 知；ri(xi, yi, zi)和 ri+1(x i+1, y i+1, z i+1)分别为永磁体中心到传 感器 i 和传感器 i+1 的矢径。即有：
R(x,y,z)为永磁体的中心矢径；r=r´–R。将矢量表达式(1)
展开为三个方向的标量表达式：
⎡ ⎢
Bx
⎢By
⎢⎣ Bz
⎤ ⎥ ⎥ ⎥⎦
=
μ0 4πr 5
·
(2)
⎡ 3(x' − x)2 − r 2 ⎢⎢3(x' − x)( y' − y)
3(x' − x)( y' − y) 3( y' − y)2 − r 2
第 11 期
王 喆 等：高背景磁场下的差分磁定位算法及应用
2385
状态测定中[6-7]。永磁体周围空间的磁场分布具有特定的规
律,当永磁体的尺寸远小于检测点和永磁体间的距离时,
可以将永磁体等效为磁偶极子。磁矩为 m 的永磁体在距
离其为 r 处产生的磁感应强度 B 的矢量表达式为[1]：
B
=
μ0 4π