CN109324298A - 一种基于检测阵列运动规划的磁源磁场信号检测方法 - Google Patents

Abstract

本发明公开了一种磁源磁场信号的检测方法，该方法预先存储外部磁体与检测阵列之间具有不同相对位姿时，检测阵列检测得到的每种相对位姿对应的外部磁体磁场；在通过外部磁体引导磁源运动的条件下，需要进行磁源定位时，将检测阵列运动到外部磁体与检测阵列的多个相对位姿中的最优相对位姿上；所述最优相对位姿的选取标准为：令磁源尽量靠近检测阵列的中心，使得与外部磁体相比，磁源磁场所占比重增大；最后，在当前最优相对位姿处，检测阵列测量总磁场；从所述总磁场中去除最优相对位姿对应的外部磁体磁场以及地磁磁场，得到磁源磁场。使用本发明能够精确去除外部磁体的磁场信号，得到有效的磁源磁场信号，为磁源精准定位提供可靠数据。

Description

一种基于检测阵列运动规划的磁源磁场信号检测方法

技术领域

本发明涉及移动磁源检测技术领域，具体涉及一种基于检测阵列运动规划的磁源磁场信号检测方法。

背景技术

外部磁体引导的主动胶囊机器人系统需要根据胶囊磁场信号定位胶囊的位姿，在外部磁体引导的主动胶囊机器人系统中存在很强的外部磁体磁场，还存在地磁场的干扰。对于外部磁场来说，其与胶囊机器人磁场相比，其所占比重较大，因此精确的去除外部磁体的磁场对胶囊位姿的确定有更为重要的意义。

如何准确去除外部磁体和地磁场的磁场信号，得到有效的胶囊磁场信号是一个重要的研究方向。

发明内容

有鉴于此，本发明提供了一种基于检测阵列运动规划的磁源磁场信号检测方法，能够去除外部磁体的磁场信号，得到有效的磁源磁场信号，为磁源精准定位提供可靠数据。

为了解决上述技术问题，本发明是这样实现的：

一种磁源磁场信号的检测方法，该方法应用于利用检测阵列跟踪外部磁体以检测磁源磁场信号的过程，包括：

步骤一、预先存储外部磁体与检测阵列之间具有不同相对位姿时，检测阵列检测得到的每种相对位姿对应的外部磁体磁场；

步骤二、在通过外部磁体引导磁源运动的条件下，需要进行磁源定位时，将检测阵列运动到外部磁体与检测阵列的多个相对位姿中的最优相对位姿上；所述最优相对位姿的选取标准为：令磁源尽量靠近检测阵列的中心，使得与外部磁体相比，磁源的磁场所占比重增大；

步骤三、在当前最优相对位姿处，检测阵列测量总磁场；从所述总磁场中去除最优相对位姿对应的外部磁体磁场以及地磁磁场，得到磁源磁场。

优选地，所述步骤一为：固定检测阵列位姿，移动外部磁体到不同的多个位姿上；利用检测阵列测量磁场，该测量磁场减去检测阵列当前位姿对应的地磁磁场，得到各个相对位姿对应的外部磁体磁场。

优选地，步骤一中，所述检测阵列当前位姿对应的地磁磁场的获取方式为：检测阵列位姿固定后，在去除外部磁体和磁源的情况下，采用检测阵列中的每个磁传感器芯片测量磁场信号，得到检测阵列当前所处的地磁磁场。

优选地，所述步骤二中，根据令磁源尽量靠近检测阵列的中心的选取标准，确定最优相对位姿的方式为：

利用磁源上一次检测位置以及外部磁体当前位置，获取磁源估计位置；

根据判定指标，选取最优相对位姿；所述判定指标包括：

①由检测阵列中心指向磁源估计位置的方向向量最接近由检测阵列中心指出的法向量方向；

②磁源估计位置与检测阵列之间的垂直距离最小。

优选地，所述最优相对位姿的选取标准进一步包括：③检测阵列从当前位姿运动至最优相对位姿时，各自由度位移绝对值之和最小。

优选地，进一步对选取最优相对位姿的各个标准进行加权融合，利用融合指标选取最优相对位姿。

优选地，所述获取磁源估计位置为：将磁源上一次检测位置和外部磁体当前位置的连线中心作为所述磁源估计位置。

优选地，当检测阵列处于某一位姿时，该位姿对应的地磁磁场的获取方式为：

预先存储检测阵列运动空间范围内各位置点的地磁磁场；

在检测阵列当前位姿下，判断检测阵列上每个磁传感器芯片的位置是否与存储磁场的各位置点重合；对于重合者，直接从存储数据中取值得到磁传感器芯片的磁场B1；对于未重合者，将存储的相邻位置点的磁场值进行插值计算，得到磁传感器芯片的磁场B1；

如果磁传感器芯片相对于预先存储地磁磁场所使用的磁传感器坐标轴有旋转，则将所述磁场B1对当前位姿下的检测阵列进行投影，得到检测阵列当前所处位姿对应的地磁磁场；如果没有旋转，则所述磁场B1即为检测阵列当前所处位姿对应的地磁磁场。

优选地，所述预先存储检测阵列运动空间范围内各位置点的地磁磁场为：

选取检测阵列上其中一个磁传感器芯片A，令磁传感器芯片A的坐标系与世界坐标系重合；在去除磁源和外部磁体的情况下，在检测阵列的运动空间范围内，令检测阵列做水平和垂直方向运动，且按照设定密度均匀移动，采集各个位置点的磁场信号，即得到检测阵列运动空间范围内各位置点的地磁磁场；所述设定密度为：相邻位置点地磁信号之差与所需获得的磁源磁场精度等数量级。

有益效果：

(1)本发明在检测磁源磁场时，将检测阵列移动到最优位姿处，并从实时检测结果中减去预存的最优位姿处的外部磁体磁场，配合地磁磁场的去除，使得最终得到的磁源磁场的噪声更小，基于该高精度磁源磁场可以获得磁源的高精度位姿定位结果；

其中，外部磁体磁场是预先测量和存储的，其值更加准确，为精确去除外部磁体磁场提供了基础；

此外，最优位姿是令磁源尽量靠近检测阵列中心的位姿，在该位姿处获得的磁场信号中，磁源部分所占比重增大，从而减小了外部磁体磁场对磁场检测结果的影响，为高精度磁源磁场检测提供了又一重要条件。

(2)本发明预先存储检测阵列移动范围内的地磁磁场，在需要时，可以根据检测阵列当前位姿进行提取和换算，不需要额外增加检测设备，减轻了地磁磁场的获取难度。而且，预先存储的地磁磁场仅包括非旋转情况下的数据，减少存储量。

(3)在确定最优相对位姿时，进一步增加了检测阵列移动代价的考虑，从而提高检测速率。

(4)外部磁体只需为驱动胶囊机器人而运动，不需要兼顾位姿检测，从而减少了外部磁体的运动。

附图说明

图1为外部磁体引导胶囊及阵列定位的装置组成示意图；

图2为本发明流程图；

图3为确定磁源估计位置示意图；

图4为根据存储地磁数据B1确定最优相对位姿对应的地磁B3的示意图；其中，(a)为不旋转情况；(b)为旋转情况。

其中，1-胶囊机器人，2-检测阵列，3-外部磁体，4-存储地磁和传感器芯片重合点，5-存储地磁和传感器芯片不重合点，6-旋转情况下某磁传感器芯片的总磁场。

具体实施方式

本发明提供了一种磁源磁场信号的检测方法，该方法应用于利用检测阵列跟踪外部磁体以检测磁源磁场信号的过程中，其基本思想是：预先存储外部磁体与检测阵列之间具有不同相对位姿时，检测阵列检测得到每种相对位姿对应的外部磁体磁场；在通过外部磁体引导磁源运动的条件下，需要进行磁源定位时，将检测阵列运动到外部磁体与检测阵列的多个相对位姿中的最优相对位姿上；在当前最优相对位姿处，检测阵列测量总磁场；从所述总磁场中去除最优相对位姿对应的外部磁体磁场以及地磁磁场，得到磁源磁场。

可见，本发明在检测磁源磁场时，将检测阵列移动到最优位姿处，并从实时检测结果中减去预存的最优位姿处的外部磁体磁场，配合地磁磁场的去除，使得最终得到的磁源磁场的噪声更小。基于该高精度磁源磁场可以获得磁源的高精度位姿定位结果。

下面结合附图对本发明实施例进行详细描述。

在本发明实施例中，将上述方案运用到胶囊内窥镜的胶囊机器人位姿检测中。胶囊机器人中设置了磁源。该实施例中使用的一种磁源磁场检测阵列装置主要包括外部磁体、胶囊机器人、检测阵列、外部磁体运动引导模块、阵列运动引导模块，其中：

外部磁体和检测阵列分别设置在胶囊机器人的上下两侧，如图1所示。外部磁体通过与胶囊机器人内部安装的小磁体(磁源)发生磁交互作用，引导胶囊机器人在检测对象的体内运动，完成检查或活检等工作；检测阵列由多个检测磁场强度信号的磁传感器芯片组成，在胶囊机器人运动的过程中检测磁场变化，继而可以通过磁场数据获得胶囊机器人的位姿。

外部磁体安装于外部磁体运动引导模块，其为外部磁体提供6个方向的运动；

检测阵列安装于阵列运动引导模块，其为检测阵列提供6个方向的运动；

基于上述硬件系统，本实施例胶囊机器人磁源磁场信号的检测方法的具体实施过程如下，参见图2，具体包括：

步骤一、预先存储检测阵列运动空间范围内各位置点的地磁磁场B1。

本步骤存储的地磁磁场用于为下面步骤21和步骤41提供检测阵列各种位姿对应的地磁磁场。为了区分，这里将该地磁磁场命名为B1。

本实施例中，为了不增加额外硬件，采用检测阵列完成地磁磁场B1的采集。具体为：移除外部磁体和胶囊机器人，选取检测阵列上其中一个磁传感器芯片A，此具体实施方式中磁传感器芯片A采用三轴磁传感器。令磁传感器芯片A的坐标系与世界坐标系重合；在检测阵列的运动空间范围内，令检测阵列做水平和垂直方向运动，并按照设定密度均匀移动，采集各个位置点的磁场信号，得到检测阵列运动空间范围内各位置点的地磁磁场。

因为地磁信号量级在几百mGs，胶囊定位要求各个轴的磁场精度在几十mGs以下，因此，存储地磁信号，要求相邻检测点地磁信号之差不超过几十mGs。也就是说，上述设定密度应该满足如下要求，即相邻位置点地磁信号之差与所需获得的磁源磁场精度等数量级。

这里，只做水平和垂直方向运动，可以减少数据存储量，如果计算检测阵列某种旋转姿态下的地磁磁场时，可以通过对存储数据的投影实现，参见步骤41。

在实际中，还可以采用额外设置的磁传感器完成上述地磁磁场B1的测量。

步骤二、预先存储外部磁体与检测阵列之间具有不同相对位姿时，检测阵列检测得到的每种相对位姿对应的外部磁体磁场。

由于是相对位姿，因此可以变化外部磁体和/或检测阵列实现。本实施例中，固定检测阵列位姿，移动外部磁体到不同的多个位姿上；利用检测阵列测量磁场，测量磁场减去检测阵列当前所处的地磁磁场，得到各个相对位姿对应的外部磁体磁场。具体步骤包括：

步骤21、固定检测阵列位姿，去除外部磁体和胶囊机器人，采用检测阵列中每个磁传感器芯片采集磁场信号，此时即为检测阵列当前位姿下的地磁磁场，记为B2。该地磁磁场B2的另外一种获取方式为利用地磁磁场B1，经过换算得到，参见步骤41。

步骤22、将外部磁体置于工作位置，再移动外部磁体到不同的多个位姿上，再利用检测阵列测量磁场，此时的测量磁场为地磁叠加外部磁体磁场的总磁场，用该总磁场减去对应的地磁磁场B2，即得到各个相对位姿对应的外部磁体磁场。这样保证了测量的磁场为外部磁体的磁场分布，排除了地磁的影响。

由于检测阵列可以跟随外部磁体运动，因此不需要存储较多的外部磁体不同位姿下检测阵列检测到的磁场信号，实际实施中，选取不少于10种不同相对位姿为佳。

步骤三、检测阵列的运动规划：

在通过外部磁体引导胶囊机器人运动的过程中，需要进行定位时，将检测阵列运动到外部磁体与检测阵列的多个相对位姿中的最优相对位姿上，将此最优相对位姿作为检测阵列检测胶囊磁场的最终检测的位姿。

本实施例中，优选地，最优相对位姿的确定原则包括两方面，一方面是令磁源估计位置尽量靠近检测阵列的中心，使得与外部磁体相比，胶囊机器人磁场所占比重增大，从而减小外部磁体磁场对胶囊机器人磁场检测结果的影响(涉及第①和②条)；第二方面是，尽量令检测阵列以最小的代价移动到检测位置处(涉及第③条)。因此，给出了如下三条判定指标：

①由检测阵列中心指向磁源估计位置的方向向量最接近由检测阵列中心指出的法向量方向；

②磁源估计位置距离检测阵列的垂直距离最小。①和②的目的是令磁源估计位置尽量靠近检测阵列中心。

③检测阵列从当前位姿运动至最优相对位姿时，各自由度位移绝对值之和最小，从而使得检测阵列移动距离最短，那么移动速度则最快，提高检测效率。

那么本步骤的运动规划包括如下两个子步骤：

步骤31：利用胶囊机器人上一次检测位置以及外部磁体当前位置，获取磁源估计位置。

优选地，如图3所示，胶囊估计位置D_I处于外部磁体当前位置和胶囊机器人上一次检测位置的连线上，D_I处于中心位置，把此D_I作为胶囊机器人估计位置。

步骤32：得到胶囊估计位置后，检测阵列按照上述判定指标找到多个位姿中胶囊定位的最佳位姿，即检测阵列和外部磁体的最优相对位姿。

上述三个判定指标中，①和②更为重要；③可以作为辅助指标。在实际中，可以对三个判定指标可以进行加权融合，得到综合指标，利用综合指标进行选取。具体来说：设检测阵列与外部磁体的相对位姿为R＝{ΔP₁,ΔP₂,…,ΔP_m,…,ΔP_M}，M是存储的相对位姿数量。为阐明融合指标，做如下定义：

是世界坐标系下外部磁体当前位姿；

其中是外部磁体当前位置，α_E,β_E,γ_E是外部磁体当前角度。

是世界坐标系下胶囊机器人上一次检测位姿；

其中是胶囊机器人位置，α_C,β_C,γ_C是胶囊机器人角度。

是世界坐标系下检测阵列位姿；

其中是检测阵列中心点位置，α_S,β_S,γ_S是检测阵列角度。

是胶囊机器人估计位置。

是检测阵列的单位法向量。

在满足的条件下，融合判定指标：

其中，θ_i是阵列运动引导模块的第i个自由度的运动位移，Δθ_i是检测阵列运动至目标位姿时，阵列运动引导模块的第i个自由度的运动位移变化量，w₁∈[0,1]，w₂∈[0,1]，w₃∈[0,1]是加权系数或调节系数，用于调节各判定指标在运动规划过程中的作用强度。最终选取融合判定指标最小者对应的相对位姿，作为最终检测位姿。

步骤四、在当前最优相对位姿处，检测阵列测量总磁场；从所述总磁场中去除最优相对位姿对应的外部磁体磁场以及地磁磁场，得到胶囊机器人磁场。

步骤41、在检测阵列处于当前位姿时，获取地磁磁场B3。

步骤一已经预先存储了检测阵列运动空间范围内各位置点的地磁磁场B1。

本步骤中，在检测阵列当前位姿下，可以根据当前位姿与存储地磁磁场B1时传感器坐标轴的姿态，分两种情况进行处理：

检测阵列运动到最优相对位姿时如果相对世界坐标系没有旋转，则每个磁传感器坐标轴仍旧和世界坐标系平行，此时检测阵列上的每个传感器芯片不是和存储磁场的位置重合，就是磁传感器芯片附近有两个或几个最近的存储磁场的位置。如果重合，如图4(a)中的重合点4(图4中灰色大点为预先存储地磁磁场的位置点)，则直接从存储数据B1中提取磁传感器芯片位置处的三轴磁场，仍记为B1，表示数据来自步骤一的存储数据；如果不重合，如图4(a)中不重合点5，则将相邻位置点的若干个磁场每个轴向的磁场分别进行插值，得到磁传感器芯片所在位置的三轴磁场，也记为B1。这样，可以得到检测阵列上所有磁传感器芯片位置的三轴磁场。由于没有旋转，则该三轴磁场B1就是所需获得的检测阵列当前位姿下的地磁磁场B3。三轴磁传感器测量的地磁磁场B3是每个磁传感器芯片对应三个数据。

如果检测阵列运动到最优相对位姿时，相对于预先存储地磁磁场时所使用的磁传感器坐标轴(这里是世界坐标系)有旋转，判断旋转之后的检测阵列上每个磁传感器芯片位置是不是和存储磁场的位置重合，如果重合则直接取值得到磁场B1，如果不重合则插值之后得到磁场B1，如图4(b)中的位置点6的总磁场所示，再把磁场B1对当前位姿下的检测阵列坐标轴进行相应的投影，得到投影后的三轴地磁场，作为当前所用的地磁磁场信号B3。

该获得地磁磁场B3的方案，也可以应用于前述步骤21中，根据地磁磁场B1获得地磁磁场B2。

步骤42、在检测阵列处于当前位姿时，检测阵列采集当前磁场信号，该磁场为总磁场D，其包含了地磁和外部磁体磁场。此时，从步骤二预先存储的数据中提取当前最优相对位姿对应的外部磁体磁场C；然后利用总磁场D减去步骤41获得的地磁磁场B3，再减去外部磁体磁场C，从而获得了胶囊机器人磁场。利用该胶囊及其磁场可以进行胶囊位姿的精准定位。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种磁源磁场信号的检测方法，该方法应用于利用检测阵列跟踪外部磁体以检测磁源磁场信号的过程，其特征在于，包括：

步骤一、预先存储外部磁体与检测阵列之间具有不同相对位姿时，检测阵列检测得到的每种相对位姿对应的外部磁体磁场；

步骤二、在通过外部磁体引导磁源运动的条件下，需要进行磁源定位时，将检测阵列运动到外部磁体与检测阵列的多个相对位姿中的最优相对位姿上；所述最优相对位姿的选取标准为：令磁源尽量靠近检测阵列的中心，使得与外部磁体相比，磁源的磁场所占比重增大；

步骤三、在当前最优相对位姿处，检测阵列测量总磁场；从所述总磁场中去除最优相对位姿对应的外部磁体磁场以及地磁磁场，得到磁源磁场。

2.如权利要求1所述的磁源磁场信号的检测方法，其特征在于，所述步骤一为：固定检测阵列位姿，移动外部磁体到不同的多个位姿上；利用检测阵列测量磁场，该测量磁场减去检测阵列当前位姿对应的地磁磁场，得到各个相对位姿对应的外部磁体磁场。

3.如权利要求2所述的磁源磁场信号的检测方法，其特征在于，步骤一中，所述检测阵列当前位姿对应的地磁磁场的获取方式为：检测阵列位姿固定后，在去除外部磁体和磁源的情况下，采用检测阵列中的每个磁传感器芯片测量磁场信号，得到检测阵列当前所处的地磁磁场。

4.如权利要求1所述的磁源磁场信号的检测方法，其特征在于，所述步骤二中，根据令磁源尽量靠近检测阵列的中心的选取标准，确定最优相对位姿的方式为：

利用磁源上一次检测位置以及外部磁体当前位置，获取磁源估计位置；

根据判定指标，选取最优相对位姿；所述判定指标包括：

①由检测阵列中心指向磁源估计位置的方向向量最接近由检测阵列中心指出的法向量方向；

②磁源估计位置与检测阵列之间的垂直距离最小。

5.如权利要求1或4所述的磁源磁场信号的检测方法，其特征在于，所述最优相对位姿的选取标准进一步包括：③检测阵列从当前位姿运动至最优相对位姿时，各自由度位移绝对值之和最小。

6.如权利要求5所述的磁源磁场信号的检测方法，其特征在于，进一步对选取最优相对位姿的各个标准进行加权融合，利用融合指标选取最优相对位姿。

7.如权利要求4所述的磁源磁场信号的检测方法，其特征在于，所述获取磁源估计位置为：将磁源上一次检测位置和外部磁体当前位置的连线中心作为所述磁源估计位置。

8.如权利要求1或2所述的磁源磁场信号的检测方法，其特征在于，当检测阵列处于某一位姿时，该位姿对应的地磁磁场的获取方式为：

预先存储检测阵列运动空间范围内各位置点的地磁磁场；

在检测阵列当前位姿下，判断检测阵列上每个磁传感器芯片的位置是否与存储磁场的各位置点重合；对于重合者，直接从存储数据中取值得到磁传感器芯片的磁场B1；对于未重合者，将存储的相邻位置点的磁场值进行插值计算，得到磁传感器芯片的磁场B1；

如果磁传感器芯片相对于预先存储地磁磁场所使用的磁传感器坐标轴有旋转，则将所述磁场B1对当前位姿下的检测阵列进行投影，得到检测阵列当前所处位姿对应的地磁磁场；如果没有旋转，则所述磁场B1即为检测阵列当前所处位姿对应的地磁磁场。

9.如权利要求8所述的磁源磁场信号的检测方法，其特征在于，所述预先存储检测阵列运动空间范围内各位置点的地磁磁场为：

选取检测阵列上其中一个磁传感器芯片A，令磁传感器芯片A的坐标系与世界坐标系重合；在去除磁源和外部磁体的情况下，在检测阵列的运动空间范围内，令检测阵列做水平和垂直方向运动，且按照设定密度均匀移动，采集各个位置点的磁场信号，即得到检测阵列运动空间范围内各位置点的地磁磁场；所述设定密度为：相邻位置点地磁信号之差与所需获得的磁源磁场精度等数量级。