基于胶囊磁定位算法稳定性的研究

　

(四川理工学院　四川　自贡　643000)

一、胶囊定位系统原型搭建及分析

磁传感器在定位精度中起至关重要作用。为保证结果准确性和可靠性，需采用灵敏度高、感应范围广、抗干扰能力强的传感器进行实验。为使结构简单方便且更好减少能耗。本课题使用数字磁传感器替代模拟磁传感器。并且把放大器、多路复用器、模拟-数字转换器都集成在数字传感器组成的电路上。整个系统的设置和可编程滤波器都可嵌入到每个数字传感器上。

对各类磁传感器进行对比选出符合本课题要求的传感器。表1-1列出可用磁传感器几个主要参数特性。传感器之间主要差别为测量范围和输出分辨率。LSM303D传感器分辨率最高，而AK09912传感器测量范围最大。为更好提高测量范围和保证更好精确度，着重考虑传感器测量范围和分辨率两个参数。因此LSM303D和AK09912被作为本课题首选。

表1　磁传感器的比较

对两种传感器的稳定性进行测试，两类传感器在相同位置相同环境下测量地磁场。经过测试LSM303D相比AK09912传感器稳定性更好[1]。最终选择稳定性更好的LSM303D传感器应用于本系统。

图1为LSM303D传感器组成平面传感器阵列。运用数字磁传感器后，传感器阵列结构更加简洁且可靠性也得到提高。

依照模型搭建系统，此系统包含数字-输出磁传感器阵列、一个ARM控制器、一个校验板和一台计算机。如图2所示，系统中传感器阵列中包含9个LSM303D传感器。九个传感器焊接在一块面积为1.5×1.5m2电路板上。传感器阵列尺寸在未来应用中可以根据需求进行增加或减少。ARM控制器STM32F103通过SPI总线读取磁传感器采集数据，并通过USB串口转UART接口传输到电脑。计算机进行位置计算，最终把磁铁位置、方向等信息实时显示在电脑屏幕。传感器阵列和ARM控制器在整个过程中共消耗455mW功耗，而传感器阵列只消耗45mW[2]。

图1　含有9个数字磁传感器的平面传感器阵列

图2　磁铁跟踪系统原型

二、磁定位原型数学模型和算法研究

(一)磁轨迹跟踪数学模型

图3　磁偶极子模型

如图3所示，(a,b,c)为磁铁中心，(xl,yl,zl)为第l个传感器位置。H0为磁铁从南极到北极的某个向量(m,n,p)T。Pl为从点(a,b,c)到点(xl,yl,zl)向量。空间中某点磁通密度Bl可以用如下公式表示[3]：

(1-1)

N为传感器总数；μr为媒介相对渗透率(空气中，μr≈1)；μ0为空气磁导率(μ0=4π·10-7T·m/A)；Rl为Pl的长度，MT为某个与磁铁相关的常数，其可用如下公式表示：

MT=πδ2LM0

(1-2)

δ表示圆形磁铁半径，L表示圆形磁铁长度，M0(单位/m)表示磁化磁铁表面。

当圆柱形磁铁沿主轴旋转时，Bl将保持不变和对称。因此不能获得旋转角度。H0长度不受磁铁方向影响，因此需添加(1-3)作为约束条件：

m2+n2+p2=1

(1-3)

(二)非线性优化算法

本课题使用两个或多个硬件磁传感器测量磁铁周围磁通强度，并运用已测得数据和已有数学模型，通过非线性优化算法计算磁铁位置参数和方向参数。通过扩展方程式(2-1)可得如下式子[4]：

(1-4)

(1-5)

(1-6)

(1-7)

求解公式(1-7)最优解常用方法有LM算法、Powell算法和DIRECT算法。结合本系统对实时性和精确性的要求，最终选择LM算法作为最优解算法。LM算法解决多元函数的方法为非线性函数迭代的平方和，融合最陡下降全局收敛和快速牛顿法局部收敛的优点。

三、传感器阵列的校准分析及收敛性研究

为提高轨迹跟踪系统精确度，每个用来标定校准的传感器都彼此独立。因为每个数字磁传感器灵敏度出厂时都已被标定，所以只需标定每个传感器位置和方向即可。

为保证校准精度，需要一个高精度校准板。校准板加工精度为0.1mm。校准初始阶段，磁铁被放置在校准板上多个比较特殊位置。磁铁位置和方向将被输出和记录。标定过程中，只阐述第l个校准传感器，其他传感器用同样方法来校准。

(一)传感器位置校验

(1-8)

公式中(Bix,Biy,Biz)T为被优化参数。校准位置可以通过将Esp做最小化处理得到。LM算法用来对Esp做最小化处理。

(二)传感器方向校准

传感器三个坐标轴不完全平行于跟踪系统中相应的坐标轴将影响定位精确性。因此本课题采用抽样数据向量变换法减小误差。

(1-9)

M为变换矩阵，在跟踪坐标系系统中把传感器采集的数据转换到三个投影坐标轴上的数据。求解矩阵M,定义客观误差函数如下[7]：

(1-10)

(三)校准方法的收敛性

传感器位置和方向通过一次计算不能获得的其最优解，需进行多次调整。根据最新M矩阵，传感器可以定位出P点位置。根据最新P点位置可以使传感器方向转化成矩阵M。直到迭代中最大值完成或者参数的相对变化足够小循环将终止。

如迭代不收敛，那么校准结果即为错。如图4所示为传感器位置校准收敛趋势，经过50次迭代后迭代将收敛。图5所示为x轴方向传感器方向校准的收敛性。经过60次迭代后迭代将收敛，相应Y轴和Z轴也有相似收敛趋势，通过校准，得到传感器位置和方向更加接近真实值[8]。

图4　融合传感器位置的校准

图5　融合传感器x轴方向的调整

四、磁定位原型系统的实验结果分析

磁轨迹跟踪系统的建立是为评测实时性、鲁棒性、位置和方向精确性。

磁铁以3cm/s速度在离传感器阵列10cm高度上进行移动，图6所示为其轨迹跟踪。它显示磁铁以20HZ频率更新运动的2D和3D轨迹数据。

图6　磁轨迹跟踪系统界面Fig.6　Magnetic trajectory tracking system interface

同时对跟踪系统的稳定性进行测试。永磁铁放在坐标为x=30mm,y=30,z=96mm位置，如图7所示为磁铁在此位置600s期间的坐标变化。测试结果得出在不同位置误差都在0.5mm之内。X轴、Y轴、Z轴标准误差分别为0.094mm，0.113mm和0.048mm。

图7　在600s内坐标上点的变化

校准板上的孔为永磁铁移动的轨迹坐标，永磁铁可以放置在预定准确位置。把磁铁分别放置在距传感器阵列高76mm和96mm的校准板上进行定位测试，最终定位值与永磁铁预定位置值如图8所示。测试结果中得到轨迹位置值和其预定值基本重合，同时验证了校验结果的准确性。

图8　三维空间实际值和测量值误差比对

从校验结果和最后测试稳定性的仿真图可以看到，在经过校验优化后的磁定位系统无论是在方向上还是在位置上精度都达到了很高的要求，符合项目要求。