基于FPGA的磁定位技术研究与实现

成龙 郑建立

摘 要：在磁定位技术应用过程中，为解决数据处理过程复杂、计算量大导致实时性无法满足问题，利用FPGA的并行结构，设计实现了一种基于FPGA平台的磁定位方案。实验结果表明，磁矩为0.1255[A？m2]的永磁体在距离磁传感器高度在20cm以内时，定位精度能达到毫米级，定位速度比通用计算机提升了一倍以上，可以满足绝大多数实时性要求，更有利于永磁体定位技术产业化，应用前景广阔。

关键词：磁定位技术;数据处理;实时性;FPGA;定位精度

DOI：10. 11907/rjdk. 182679 开放科学（资源服务）标识码（OSID）：

中图分类号：TP319文献标识码：A 文章编号：1672-7800（2019）007-0145-05

Research and Implementation of Magnetic Positioning Technology Based on FPGA

CHENG Long， ZHENG Jian-li

（School of Medical Instrument and Food Engineering， University of Shanghai for Science and Technology， Shanghai 200093， China）

Abstract： In the process of applying magnetic positioning technology， the complicated data processing and the large calculation amount often lead to that the real-time requirement cannot be met.In order to solve this problem， based on the parallel structure of FPGA， this study designs and implements a magnetic positioning scheme， including data acquisition，data processing and data output.The results show that the positioning accuracy can reach millimeter level when the height of the magnet with a magnetic moment of 0.1255[A？m2] is within 20 cm from the magnetic sensor. Moreover， the positioning speed of the scheme is more than double that of using the general computer， which can satisfy most real-time requirements.It is more beneficial to the commercialized development of magnetic positioning technology and has broad application prospects.

Key Words： magnetic positioning technology; data processing; real-time; FPGA; positioning accuracy

基金项目：上海市科委科技支撑项目（17-C-2）;上海理工大学微创励志创新基金项目（YS30810128）

作者简介：成龙（1994-），男，上海理工大学医疗器械与食品学院硕士研究生，研究方向为医学仪器嵌入式技术;郑建立（1965-），男，博士，上海理工大学医疗器械与食品学院副教授、硕士生导师，研究方向为医学信息系统与集成技术、医学仪器嵌入式控制系统。本文通讯作者：郑建立。

0 引言

磁定位技术是目前应用较为广泛的定位技术之一，适用于空间较小、无磁干扰的场景，具有定位精度高、成本较低等优点。磁定位技术分为电磁定位技术和永磁定位技术[1]。电磁定位技术主要由磁场发射源、磁场接收、数据处理3部分组成。向三轴线圈通入交流或直流脉冲信号，线圈周围产生的感应磁场作为磁场发射源。磁传感器作为磁场接收装置，对采集到的信号进行处理得到六维数据[2]。电磁定位技术的优点是不受视线阻挡，缺点是采用直流或低频电流作为激励，控制电路伴有能耗，环境中的铁磁性物质会造成电磁场畸变从而影响精度。永磁定位技术由永磁体、磁传感器阵列、数据处理3部分组成。通过高精度的磁傳感器组成的传感器阵列，采集永磁体的三轴磁感应强度值，然后将采集到的三轴数据进行定位算法处理，得到磁体在空间中的三维坐标。永磁体定位技术优点在于无需供电和控制电路，无需激励电流。现阶段永磁体多采用强磁材料钕铁硼制成，具有体积小、磁性强的特点，使永磁定位技术信号源占用空间小，在微型化和功耗要求高的应用环境中优势明显。

永磁体定位技术研究很多，如侯文生等[3]建立了一套针对消化道微型诊查设备的定位系统，胡超等[4]提出了一种基于人工蜂群算法的定位方式，刘畅等[5]建立了一套胶囊内窥镜弱磁场测量系统，任宇鹏等[6]修正了磁偶极子模型在近场不适用问题。以上研究在数据处理部分大多依赖Matlab和Labview等PC端软件完成，处理速度较慢、实时性不够。数据依赖PC处理不方便携带，难于产业化。

计算机数据处理往往是基于一种串行结构，而FPGA内部由LUT和FF构成，拥有丰富的时序逻辑资源，现在很多FPGA都集成了DSP和BRAM。因此，常常利用FPGA处理一些数据量较大、过程较为复杂的算法。将算法中的多个过程完全并行处理，在同一段时间内就能进行多路计算，大大加快了数据处理过程[7]。

1 磁定位原理

1.1 磁偶极子模型

磁偶极子模型广泛应用于目标物体的定位和状态测定中。当目标磁体的体积远远小于检测距离时，该物体可看作磁偶极子。磁偶极子是最小的磁单元，它是类比电偶极子而建立的模型[8]，由大小相等、方向相反的两个点磁荷构成系统。自然界有许多磁现象可以视为一个磁偶极子，比如一根小的磁针。

磁偶极子模型中磁感应强度计算公式如下：

[B=BT（3（G？X）XR5-GR3）BT=μ0M04π]       （1）

其中，[M0]是磁体的磁矩大小，可通过实验直接测得已知量。[μ0]是空间中的磁导率，为一个已知量[9]。[G]是磁矩的单位方向向量，可表示为：[G=[sin？cosφ，sin？sinφ，][cos？]T]。其中，[？]和[φ]是未知量，[？]为[G]与Z轴正方向的夹角，[φ]为[G]在XY平面上的投影与X轴正方向的夹角[10-11]。[X]是磁体到磁传感器的矢径。假设永磁体在空间中的坐标为[[a，b，c]]，这是个未知量，磁传感器在空间中的坐标为[[x0，y0，z0]]，是已知量，则[X=[x0-a，y0-b，z0-c]T]。[R]是磁体到磁传感器的距离，上述磁感应强度计算公式中有5个未知的待优化参数，分别是[a，b，c，？，φ]。

1.2 定位算法原理

磁定位的最终目标是找到磁体在空间中的三维坐标和角度信息，相当于求解出这5个参数的最优解，使得最优解所对应的磁感应强度计算公式的函数值最接近磁体在空间中的真实磁感应强度。因此，需要通过分辨率较高的磁传感器测量磁体在空间中的真实磁感应强度。未知参数有5个，而每个磁传感器只能提供3个关系式。从解方程的角度看，至少需要两个磁传感器才能求出5个未知参数的解，这就是为何需要磁传感器阵列而不是单个磁传感器测量磁感应强度的原因。磁传感器数量越多，所能提供的约束条件也越多，求解出的最优解就越精确。

求解最优解的过程其实是一种参数优化过程，所以需要用到最优化算法。常见的最优化算法有牛顿法、高斯—牛顿法、梯度下降法和L-M算法等。牛顿法是最早应用的优化算法之一，缺点在于只适用于变量是一维的情况。高斯—牛顿法和梯度下降法在牛顿法的基础上加入了Hessian矩阵，用Hessian矩阵表示高维的求导函数，可以处理多维变量[12]。L-M算法又在梯度下降法和高斯—牛顿法基础上引入一个加权因子，记为λ。该加权因子的作用是控制接近最优解的速度。若速度太快，就使用较小的λ，使L-M算法更接近高斯—牛顿法。若速度太慢，就使用较大的λ，使L-M算法更接近梯度下降法[13]。加权因子的加入在增加L-M算法灵活性的同时，也让L-M算法兼备了高斯—牛顿法和梯度下降法的优点，这是L-M算法能快速收敛到全局最优解的重要原因[14]。

式（2）是L-M算法的迭代公式，其中[H（xn）]代表Hessian矩阵，[？f（xn）]代表残差函数的梯度，λ代表加权因子。

[xn+1=xn-（H（xn）+λ？diag（H（xn）））-1？？f（xn）]     （2）

根据式（2）可知，L-M算法作为一种迭代优化算法，在每一次迭代步长的求解中都需要经过以下几个步骤：①求Hessian矩阵和加权因子λ;②将Hessian矩阵的对角线元素加权;③对上一步处理完的结果求逆，并与残差函数的梯度相乘。

具体的L-M算法参数优化过程为：①给5个参数分别赋一个初值，将它们代入磁感应强度计算公式中，得到一个计算值，记为[B1];②通过磁传感器阵列去采集永磁体在空间中的磁感应强度，记为[Bt];③如果磁传感器的精度足够高，就可认为测得的[Bt]是一个相对精确的值。一开始，[B1]通常是偏离[Bt]较远的，通过多次迭代，不断缩小[B1]和[Bt]的差值，直到两者误差小于设定的阈值，就可近似认为此函数中未知数的值即是最优解，也即所要定位的永磁体的坐标信息和角度信息。

2 总体方案

2.1 系统架构

本设计由数据采集、数据处理和结果输出3部分组成，流程如图1所示。数据采集部分主要通过FPGA的I/O模拟磁传感器的IIC数据接口，对磁传感器的内部寄存器进行读写操作。控制传感器对永磁体的三轴磁感应强度进行测量，并在测量完毕后读出数据，交给定位算法部分处理。数据处理部分采用Lenvenberg-Marquard算法（常简称为L-M算法）。最后将数据处理结果通过串口输出，在PC端界面上显示定位结果。

图1 方案流程

2.2 数据采集部分

本设计采用的磁传感器是Maxim公司的各向异性磁阻传感器MMC5883MA。各向异性磁阻传感器的测量范围以地球磁場分布范围为中心，是最适合工作在地球磁场环境下的磁传感器，其分辨率为0.25mGauss，量程为[±8]Gauss，最低采样频率为100Hz，A/D转换精度为16bit，输入输出接口为IIC。其出色的性能可为软硬件的抗干扰修正提供更为快速的算法，带来更为精确及快速的方向定位。

该传感器集成了IIC的数据接口，用户可通过该接口配置传感器的内部寄存器，从而控制传感器测量永磁体的三轴磁感应强度，并在测量完成后读出测量数据。

磁传感器的控制过程如图2所示。每次上电后都需要对传感器进行置位和复位操作，随后等待大约1～2ms，接着给传感器发送测量指令，然后再等待10ms时间就可从传感器的内部寄存器中读取有效测量数据。若要进行连续测量，可在读完数据之后立刻发送再次测量指令，但每次都需要等待10ms之后才能读到有效数据。

图2 磁传感器控制过程

2.3 数据处理部分

数据处理部分涉及的L-M算法各功能模块如图3所示。该算法的实现分为目标函数值计算块、Hessian矩阵生成块、矩阵求逆块、加权因子λ计算块、迭代控制块和矩阵乘法块几个部分。

目标函数计算块中的目标函数是磁偶极子模型中磁感应强度计算公式，该计算块的输入值由两部分组成：①数据采集模块输出的永磁体三轴磁感应强度，即为图3中的测量值;②每个待优化参数的初始值，该初始值可任意给定。经过目标函数计算块之后，得到测量值与计算值的残差函数及方差和。迭代控制块根据方差和是否小于阈值判断迭代是否继续。若方差和小于设定阈值则迭代完成，大于阈值则迭代继续。在迭代继续的情况下，Hessian矩阵生成块和加权因子计算块会同时工作，分别计算Hessian矩阵和加权因子λ，计算完成后将两者同时作为输入传到矩阵求逆块。矩阵求逆块负责对角加权后的Hessian矩阵求逆，并将求逆后的矩阵、Hessian矩阵以及残差函数一起传到矩阵乘法块，计算下次迭代的步长[dx]，并将初值与[dx]的差作为下次迭代的输入值再次进行迭代，直到迭代完成。

图3 L-M算法功能模块

2.4 结果输出部分

结果输出部分将算法处理完成并输出的数据通过串口传给PC，并在PC端编写串口接收界面显示磁体的三维坐标和角度信息。其中，串口波特率为115200bps，1位停止位，8位数据位，无校验位。串口输出的数据帧格式如图4所示，以0x02作为帧头，紧接着输出6个有效数据，每个有效数据都以4字节的ASCII码表示。有效数据之后是CRC校验码，CRC校验码是4字节的ASCII码，最后以0x03作为帧尾。

[0x02＼&X轴坐标值＼&Y轴坐标值＼&Z轴坐标值＼&弧度信息1＼&弧度信息2＼&迭代次数＼&CRC校验码＼&0x03＼&]

图4 串口输出的数据帧格式

3 数据处理模块

3.1 Hessian矩阵生成块

Hessian矩阵又称为黑塞矩阵，是一个由多元函数的一阶偏导和二阶偏导构成的对称正定矩阵[15]，描述了函数的局部曲率。黑塞矩阵最早由德国数学家Ludwig Otto Hesse提出，并以其名字命名。对于一个多元函数[f（x1，x2）]，黑塞矩阵的计算公式如下：

[H（x1，x2）=2i=1n（？fi？x1？fi？x2+fi？2fi？x1？x2）]     （3）

在Hessian矩阵中，由于计算复杂在L-M算法中常常省略二阶偏导部分，仅对函数作一阶偏导处理，硬件实现一阶偏导同样非常复杂。本设计采用有限差分法近似替代一阶偏导。差分法是微分方程的一种近似数值解法，相当于用有限差分代替微分，用有限商分代替导数，因此求解微分方程的问题就转换为求解代数方程问题[16]。在简化运算提升效率的同时误差也相对较小，以函数[f（x）]为例，[f（x）]的一阶导数[f（x）]可用公式（4）表示。

[f（x）=f（x+h）-f（x）h]    （4）

3.2 加权因子λ计算块

加权因子λ值的确定采用信赖域法，信赖域法基本思想是：先给定一个初始的λ值作为位移上界，以初始值点为圆心，以此位移上界为半径所画出的闭球区域作为信赖域区间，通过在此区间求解信赖域子问题的最优解得到一个位移量。若该位移有充分的下降，就保持或扩大该信赖域半径，并且以位移后的新点作为圆心重新确定信赖域[17]。若该位移的下降不够充分或者根本没有下降则圆心不变，缩小信赖域，重新求解该信赖域子问题的最优解得到新的候选位移。如此重复下去，直到满足迭代终止条件为止[18]。

本设计给定的降幅区间为[0.25，0.75]。所谓降幅即为实际下降量与预测下降量的比值，简单说降幅的作用就是监督当前步长的质量。若当前降幅在区间内就满足当前步长，保持目前的信赖域半径不变，接受该步长。若当前降幅超出该区间則说明当前步长质量很高，可以接受该步长并扩大信赖域半径再次迭代。若当前降幅小于该区间，说明当前步长质量不佳，就缩小信赖域重新进行搜索。

3.3 矩阵求逆块

常用的矩阵求逆算法有LU分解法、QR分解法、Cholesky分解法等[19]。LU分解法适用于一切可逆矩阵的求逆，优点是具有普遍性，缺点是过程较为复杂，在硬件实现时消耗的逻辑资源较多，耗时也较长，效率不高。Cholesky分解法仅仅适用于对称正定矩阵，普遍性不如LU分解法，优点是硬件实现时消耗的逻辑资源较少，耗时较短，效率较高[20]。

图5为在Xilinx Zynq7020平台上，分别采用Cholesky分解法与LU分解法设计5行5列64位矩阵求逆块综合后的逻辑资源用量对比，可以看出，Cholesky分解法所消耗的逻辑资源明显少于LU分解法，LUT资源减少了22%，FF资源减少了10%，DSP资源减少了16%。

表1为两个模块分别对同一矩阵求逆所消耗的时间，可以看到，在同样为50MHz的时钟频率下，采用LU分解法设计的求逆块大约需要50.53μs才能完成一个5行5列矩阵的求逆，而采用Cholesky分解法设计的求逆块仅需29.17μs即可完成，采用Cholesky分解法效率提升了大约42%。

因Hessian矩阵为对称正定矩阵，所以本设计采用Cholesky分解法设计矩阵求逆块。

表1 LU分解法与Cholesky分解法算法耗时比较         （μs）

图5 LU分解法与Cholesky分解法资源消耗量对比

4 结果分析

测试实验使用的永磁体类型为钕铁硼永磁体，直径为8mm，高度为3mm，磁矩为0.1255[A？m2]。

图6是采用Xilinx公司的集成开发环境Vivado对数据处理的仿真结果，可以看到，最后迭代了36次完成了5个未知参数[a，b，c，？，φ]的优化。在迭代完成后，Iterate_complete标志位置为1，随即输出满足迭代完成条件的最优解。

图7是定位信息在串口接收界面上显示的结果，以第一行数据为例：a：23 b：125 c：73 [θ]：1819 [φ]：49026 num：67。其中，23，125，73分别为X轴、Y轴和Z轴上的磁体坐标信息，单位为mm。随后的两个为磁体弧度信息，这两个值均需除1 000并转换到[-[π]，[π]]的区间内才有效。最后一个是迭代次数信息。

图8是定位误差统计，可以看到磁体距离传感器的高度是影响定位误差的重要因素。

当磁体高度在5cm左右时，平均定位误差仅在3mm左右;当磁体高度在10cm左右时，平均误差在5mm左右。随着磁体高度的不断增大，误差也会随之快速变大。当磁体距离传感器的高度达到20cm以上后，定位误差也增大到厘米级，这符合磁偶极子模型公式中磁感应强度与距离的三次方成反比的关系。

图9是分别使用FPGA和通用计算机进行20次定位所得的耗时对比，图中三角形点表示计算机的定位耗时，方形点表示FPGA的定位耗时。每次测试都给算法赋相同的初值，并且确保每次用两者进行测试时磁体的位置保持不变，这相当于确保了所有外部条件不变，从而能客观地比较两者定位耗时的差异。可以明显看出，FPGA在同样条件下的定位速度比计算机快大约一倍以上。在处理5个参数的高次目标函数优化时，平均定位耗时仅6.034ms，远低于计算机的平均定位耗时16.682ms，可以满足大多数实时性要求较高的场景。

图7 定位结果信息显示

图8 定位误差统计

图9 FPGA与通用计算机定位耗时对比

5 结语

本文描述了一种基于FPGA平台的磁定位方案，利用FPGA并行结构快速采集和处理数据，完成定位工作。使用Xilinx公司的Vivado集成开发环境对该方案进行仿真，验证该方案的正确性。最后将生成的bit文件下载到Xilinx Zynq7020开发板上，连接传感器阵列，通过串口将定位结果在PC端接收界面上显示。结果表明，在永磁体距离磁传感器20cm以下时，定位误差均在毫米级。与通用计算机进行数据处理方案相比，定位耗时缩短了一倍以上，取得了较为理想的定位效果。

本设计可有效解决磁定位过程中由于数据处理较为复杂且数据处理量大导致的数据处理时间较长问题，满足大多数实时性要求。利用FPGA取代计算机进行数据采集和处理，有利于永磁体定位技术的产品化，应用前景良好。

参考文献：

[1] 张可，汤福南，李修寒，等. 基于永磁定位技术的三维腿部运动检测系统设计[J]. 中国医疗设备，2017（9）：33-39.

[2] 包建孟. 基于电磁跟踪技术的室内移动机器人定位系统[D]. 宁波：宁波大学，2014.

[3] 候文生，郑小林，彭承琳，等. 基于永磁体空间磁场检测的体内微型诊疗装置定位系统的研究[J]. 北京生物医学工程，2004，23（2）：81-83.

[4] 郑子昭，何小其，胡超. 基于人工蜂群算法的胶囊内窥镜位姿磁定位研究[J]. 集成技术，2014（5）：52-60.

[5] 刘畅，刘修泉，黄平. 胶囊内窥镜弱磁场测量系统设计与实验研究[J]. 现代制造工程，2016（7）：95-100.

[6] 任宇鹏. 基于磁传感器阵列的无线胶囊内窥精确定位技术[D]. 太原：太原科技大学，2016.

[7] 刘志成，祝永新，汪辉，等. 基于FPGA的卷积神经网络并行加速结构设计[J]. 微电子学与计算机，2018（10）：80-84.

[8] 江胜华，侯建国，何英明，等. 基于磁偶极子的磁场梯度张量局部缩并及试验验证[J]. 中国惯性技术学报，2017（4）：473-477.

[9] 王延花. 基于磁阻传感器的磁性纳米颗粒检测系统设计[D]. 南京：南京医科大学，2014.

[10] WANG S S，GUO Q. Calculation and correction of magnetic object positioning error caused by magnetic field gradient tensor measurement[J]. Journal of Systems Engineering and Electronics，2018（3）：456-461.

[11] GAO X，LI B. A novel method of localization for moving objects with an alternating magnetic field[J]. Sensors（Basel），2017（4）：158-161.

[12] 王建梅，覃文忠. 基于L-M算法的BP神經网络分类器[J]. 武汉大学学报：信息科学版，2005（10）：928-931.

[13] 申雅峰，张磊，胡春艳. 一种基于FPGA的光纤光栅高速解调算法[J]. 光通信研究，2014（2）：39-42.

[14] 耿传飞，俞醒. 基于L-M算法的神经网络在环境振动分析中消除本底振动的应用[J]. 振动与冲击，2016（13）：14-19.

[15] 刘梅，刘红卫，杨善学，等. 基于近似Hessian矩阵的修正网格自适应直接搜索算法[J]. 南京理工大学学报，2018（2）：189-194.

[16] 薛浩，刘洋，杨宗青. 基于优化时空域频散关系的声波方程有限差分最小二乘逆时偏移[J]. 石油地球物理勘探，2018（4）：652-753.

[17] 朱红兰，倪勤，党创寅，等. 求解无约束优化问题的分式模型信赖域算法[J]. 中国科学：数学，2018（4）：531-546.

[18] 胡梦英. 一种改进的自适应信赖域算法[J]. 中国科技信息，2016（17）：80-82.

[19] 胡天驰. 基于FPGA的矩阵算法IP核技术研究[D]. 杭州：浙江大学，2016.

[20] 王禹. 基于FPGA的矩阵求逆IP核设计技术及其实验平台设计[D]. 杭州：浙江大学，2016.

（责任编辑：杜能钢）