CN110515016A - 一种磁驱动机器人有效磁场测量方法和装置 - Google Patents

Abstract

本发明公开了一种磁驱动机器人有效磁场测量方法和装置，该方法首先测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即固定偏移然后移除磁驱动机器人，将驱动磁体和磁传感器固定在磁驱动机器人工作环境中，采用磁传感器测量磁场读数，减去所述固定偏移获得环境磁场在磁驱动机器人运动过程中，磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。使用本发明能够在复杂环境中得到精确有效的磁驱动机器人磁场信号，从而进行准确的磁驱动机器人定位。

Description

一种磁驱动机器人有效磁场测量方法和装置

技术领域

本发明涉及移动磁源检测技术领域，尤其涉及磁驱动机器人有效磁场测量方法和装置。

背景技术

外部磁体(也称驱动磁体)引导磁驱动机器人(例如胶囊机器人)时，需要测量有效的胶囊磁场进行胶囊定位，然而，在整个工作空间中，存在诸多的干扰磁场，包括驱动磁体磁场、地磁磁场等多个干扰磁场源；不仅如此，磁传感器芯片还存在初始偏置。在机器人检测与控制过程中，这些干扰严重的影响了有效磁场信号的测量。

因此，需要精确测量各个磁场信号，同时准确的去除这些干扰，才能得到复杂环境中精确有效的胶囊磁场信号，最后准确的进行胶囊定位。

发明内容

有鉴于此，本发明提供了一种磁驱动机器人有效磁场测量方法和装置，能够在复杂环境中得到精确有效的磁驱动机器人磁场信号，从而进行准确的磁驱动机器人定位。

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

一种磁驱动机器人有效磁场测量方法，所述磁驱动机器人通过与驱动磁体进行磁交互，获得期望的运动；驱动磁体与磁传感器具有固定的相对位姿，该方法包括：

步骤一、测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即

步骤二、测得环境磁场

移除磁驱动机器人，将驱动磁体和磁传感器固定在磁驱动机器人工作环境中，采用磁传感器测量磁场读数，减去所述固定偏移获得环境磁场

步骤三、有效磁场测量：

在磁驱动机器人运动过程中，磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

优选地，所述步骤一为：将驱动磁体和磁传感器放置到屏蔽室中，磁传感器读数即为固定偏移

优选地，所述步骤一为：

步骤101：将磁传感器放入屏蔽箱内，磁传感器读数即为磁传感器初始固定偏移磁场

步骤102：去掉屏蔽箱，在没有磁驱动机器人和驱动磁体的条件下，令磁传感器固定在某个位姿A，获得磁传感器读数

步骤103：令磁传感器和驱动磁体固定在所述位姿A，再测一次，获得磁传感器读数

步骤104：令获得进一步得到

优选地，所述步骤一为：

步骤1A：在无屏蔽的正常工作环境中，将磁传感器放置在任一位置处，测量一次磁场，得到此磁场包括外部环境磁场和磁传感器初始固定偏移磁场 在同一位置处，磁传感器按照轴向旋转180度，再测量一次磁场，得到

步骤1B：利用和两个公式相加求出磁传感器轴向上的依次求出磁传感器不同轴的

步骤1C：在没有磁驱动机器人及驱动磁体的条件下，令磁传感器固定在某个位姿B，获得磁传感器读数

步骤1D：加入驱动磁体，在同一位姿B再测量一次，获得磁传感器读数

步骤1E：令获得进一步得到

优选地，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间所在的环境中指定位姿处的环境磁场；则步骤二在所述指定位姿处获得环境磁场。

优选地，磁驱动机器人所在运动空间任意测量点的环境磁场采用该测量点所在的子空间的环境磁场；所述子空间是通过对运动空间进行划分得到的；则步骤二在每个子空间的指定位姿处获得环境磁场。

优选地，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间内离散测量点拟合出的磁场函数曲线所求的环境磁场；则步骤二在多个位姿处获得环境磁场，通过拟合得到磁场函数曲线。

本发明还提供了一种磁驱动机器人有效磁场测量装置，包括磁驱动机器人、驱动磁体、检测模块、驱动磁体运动引导模块和控制模块；检测模块内包含磁传感器及其信号采集处理部分；驱动磁体与磁传感器具有固定的相对位姿；所述磁驱动机器人通过与驱动磁体进行磁交互，获得期望的运动；

控制模块包括偏移测量控制子模块、环境磁场测量控制子模块、有效磁场测量控制子模块；

偏移测量控制子模块，配置用于控制磁传感器测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即固定偏移

环境磁场测量控制子模块，配置用于通过驱动磁体运动引导模块将驱动磁体和测量模块中的磁传感器移动到磁驱动机器人工作环境中，在移除磁驱动机器人的情况下，控制磁传感器测量磁场数据，该数据减去所述固定偏移获得环境磁场

有效磁场测量控制子模块，配置用于在磁驱动机器人运动过程中，将磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

优选地，所述偏移测量控制子模块获得固定偏移的方式为：

在驱动磁体和磁传感器放置于屏蔽室的情况下，从磁传感器读取测量数据即为固定偏移

或者，在磁传感器放置于屏蔽箱的情况下，从磁传感器获取测量读数，该读数为磁传感器初始固定偏移磁场在去掉屏蔽箱、没有磁驱动机器人和驱动磁体的条件下，磁传感器固定在某个位姿A，从磁传感器获取测量读数在磁传感器和驱动磁体固定在所述位姿A的情况下再测一次，获得磁传感器读数令获得进一步得到

或者，在无屏蔽的正常工作环境中，磁传感器放置在任一位置处，控制磁传感器测量一次磁场，得到此磁场包括外部环境磁场和磁传感器初始固定偏移磁场 在同一位置处，通过驱动磁体运动引导模块控制磁传感器按照轴向旋转180度，再测量一次磁场，得到 利用和两个公式相加求出磁传感器轴向上的依次求出磁传感器不同轴的在移除磁驱动机器人及驱动磁体的情况下，令磁传感器固定在某个位姿B，获得磁传感器读数 在加入驱动磁体的情况下，在同一位姿B再测量一次，获得磁传感器读数 令获得进一步得到

优选地，环境磁场测量控制子模块在获取环境磁场时：

磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间所在的环境中指定位姿处的环境磁场；

或者，磁驱动机器人所在运动空间任意测量点的环境磁场采用该测量点所在的子空间的环境磁场；所述子空间是通过对运动空间进行划分得到的；

或者，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间内离散测量点拟合出的磁场函数曲线所求的环境磁场。

有益效果：

本发明有效磁场的测量去除了磁传感器初始固定偏移磁场驱动磁体固定偏移磁场和环境磁场的干扰，从而从各种背景干扰中精确的找出有效的磁场信号，进一步得到磁驱动机器人的准确位置与姿态。

而且，本发明能够准确的测量区分各个背景环境的磁场，有利于磁驱动机器人的准确定位。

附图说明

图1为本发明方法流程图；

图2为运动空间任意测量点的环境磁场求法示意图；其中(a)为单点位姿处磁场代替整个运动空间环境磁场；(b)为多点位姿处磁场代替运动空间环境磁场；(c)为用拟合函数曲线求运动空间环境磁场；

图3为无屏蔽环境传感器固定偏移磁场测量法；

其中，1，2-同一个传感器同一位置旋转180度结果，3-外部磁场，4-传感器初始偏移磁场。

具体实施方式

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

如图1所示，本发明提供了一种磁驱动机器人有效磁场测量方法，驱动磁体与磁传感器具有固定的相对位姿，该方法包括：

步骤一、测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即固定偏移

步骤二、测得环境磁场

移除磁驱动机器人，将驱动磁体和磁传感器固定在磁驱动机器人工作环境中，采用磁传感器测量磁场读数，减去所述固定偏移获得环境磁场

步骤三、有效磁场测量：

在磁驱动机器人运动过程中，磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

其中，步骤二获得的环境磁场可以有三种获得方式，下面举三个实施例来说明这三种方式。同时针对步骤一，三个实施例还分别给出的三种不同的实现方案，这三种步骤一的实现方案均适用于所有的实施例。三个实施例均以胶囊机器人作为控制对象。

实施例一

本实施例中，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间所在的环境中指定位姿处的环境磁场。

步骤一、固定偏移测量——该方案采用屏蔽室方案。

磁传感器选用3轴磁传感器，使用屏蔽室消除环境磁场影响。把驱动磁体和检测模块放置到屏蔽室中，检测模块中每个磁传感器的读数即为磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场 之和，即固定偏移

其中，N为检测模块中的磁传感器的数目，N个磁传感器得到3N个磁场数据。

步骤二，环境磁场测量——

首先，移除胶囊机器人，将驱动磁体固定在磁驱动机器人工作环境的1个特定位姿处，如图2中的(a)图三角标记处，用此位姿处的磁场代替整个运动空间环境磁场。

使用1个磁传感器进行测量，读数记为进而获得该位姿处的环境磁场为1≤j≤3，j代表3个轴，环境磁场为运动空间所在的环境中的统一坐标系下的磁场。

步骤三、有效磁场测量——

将环境磁场转换到传感器芯片坐标系下，得到在驱动磁体带动胶囊机器人运动的过程中，胶囊机器人的有效磁场数据为：

上式中，为磁传感器测量的磁场值；这里1≤i≤3N，N为检测模块中磁传感器器的个数。

当环境改变后，本步骤二需要重复进行，步骤一则是不变的前提下，一次设置即可。

实施例二

本实施例中，磁驱动机器人所在运动空间任意测量点的环境磁场采用该测量点所在的子空间的环境磁场；所述子空间是通过对运动空间进行划分得到的。

步骤一、固定偏移测量——该方案采用屏蔽箱方案。

磁传感器选用3轴磁传感器，使用屏蔽箱消除环境磁场影响。

首先将磁传感器放入屏蔽箱内，磁传感器读数即为初始固定偏移磁场N为检测模块中的磁传感器数目。

去掉屏蔽箱，在没有胶囊机器人和驱动磁体的条件下，驱动磁传感器固定在某个设定位姿A，获得磁传感器读数

之后，加入驱动磁体，令磁传感器和驱动磁体一起固定在同一位姿A，再测一次，获得磁传感器读数

令获得进一步得到

步骤二，环境磁场测量——

首先，移除胶囊机器人，将运动空间划分为M个子空间，且每个子空间可用该子空间中的特定位姿处的环境磁场代表该子空间内所有测量点的环境磁场，参见图2中(b)图的原点。驱动磁传感器移动并固定在各子空间的特定位姿，使用单个磁传感器测量，获得M个读数进而获得M个位姿处的环境磁场：M个1≤j≤3，j代表3个轴。将得到的地磁磁场数据转换至运动空间所在的环境中的统一坐标系下，获得环境磁场的表达

步骤三、有效磁场测量——

在驱动磁体带动胶囊机器人运动的过程中，胶囊机器人的有效磁场数据为：1≤i≤3N。

其中，运动空间所在的环境中的任意测量点处的磁场数据为该测量点所在的子空间的磁场数据坐标系转化而来。

当环境改变时，步骤二需要重复进行，步骤一则是不变的前提下，一次设置即可。

实施例三

本实施例中，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间内离散测量点拟合出的磁场函数曲线求出的磁场。

步骤一、固定偏移测量——该方案采用翻转抵消方案。

磁传感器选用3轴磁传感器。

在无屏蔽的正常工作环境中，如图3所示，将磁传感器放置在任一位置处，测量一次磁场，得到此磁场包括外部环境磁场和初始偏移值 在同一位置处，磁传感器按照轴向旋转180度，再测量一次磁场，得到 利用两个公式求出此芯片此轴向上的依次求出多个磁传感器不同轴的

移除胶囊机器人及驱动磁体，驱动磁传感器固定在某个特定位姿，获得磁传感器读数同一特定位姿，含有驱动磁体的条件下，再测一次，获得传感器读数令获得进一步得到(1≤i≤3N)

步骤二，环境磁场测量——

首先，移除胶囊机器人，驱动磁传感器固定在N个特定位姿上，使用多个磁传感器中的一个磁传感器，获得多个读数1≤j≤3，j代表3个轴，进而获得该位姿处的环境磁场，将得到的环境磁场数据转换至运动空间所在的环境中的统一坐标系下，之后，拟合出环境磁场在此空间中的函数曲线，获得环境磁场的表达 为运动空间内任意位置的环境磁场。

步骤三、有效磁场测量——

在驱动磁体带动胶囊机器人运动的过程中，首先利用磁场函数曲线求出当前每个磁传感器所在位置的不同轴的环境磁场，再利用公式得到胶囊机器人的有效磁场数据：

1≤i≤3N，N为磁传感器个数。

为了实现上述方案，本发明提供了一种磁驱动机器人有效磁场测量装置，包括磁驱动机器人、驱动磁体、检测模块、驱动磁体运动引导模块和控制模块；检测模块内包含磁传感器及其信号采集处理部分；驱动磁体与磁传感器具有固定的相对位姿；所述磁驱动机器人通过与驱动磁体进行磁交互，获得期望的运动。

控制模块包括偏移测量控制子模块、环境磁场测量控制子模块、有效磁场测量控制子模块；

偏移测量控制子模块，配置用于控制磁传感器测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即固定偏移

环境磁场测量控制子模块，配置用于通过驱动磁体运动引导模块将驱动磁体和测量模块中的磁传感器移动到磁驱动机器人工作环境中，在移除磁驱动机器人的情况下，控制磁传感器测量磁场数据，该数据减去所述固定偏移获得环境磁场

有效磁场测量控制子模块，配置用于在磁驱动机器人运动过程中，将磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

偏移测量控制子模块获得固定偏移的方式可以是上述实施例一～三中的任意一种。

环境磁场测量控制子模块在获取环境磁场时也有三种方式，分别对应上述实施例一～三中的三种。

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

Claims (10)

1.一种磁驱动机器人有效磁场测量方法，所述磁驱动机器人通过与驱动磁体进行磁交互，获得期望的运动，其特征在于，驱动磁体与磁传感器具有固定的相对位姿，该方法包括：

步骤一、测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即

步骤二、测得环境磁场

移除磁驱动机器人，将驱动磁体和磁传感器固定在磁驱动机器人工作环境中，采用磁传感器测量磁场读数，减去所述固定偏移获得环境磁场

步骤三、有效磁场测量：

在磁驱动机器人运动过程中，磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

2.如权利要求1所述的方法，其特征在于，所述步骤一为：将驱动磁体和磁传感器放置到屏蔽室中，磁传感器读数即为固定偏移

3.如权利要求1所述的方法，其特征在于，所述步骤一为：

步骤101：将磁传感器放入屏蔽箱内，磁传感器读数即为磁传感器初始固定偏移磁场

步骤102：去掉屏蔽箱，在没有磁驱动机器人和驱动磁体的条件下，令磁传感器固定在某个位姿A，获得磁传感器读数

步骤103：令磁传感器和驱动磁体固定在所述位姿A，再测一次，获得磁传感器读数

步骤104：令获得进一步得到

4.如权利要求1所述的方法，其特征在于，所述步骤一为：

步骤1A：在无屏蔽的正常工作环境中，将磁传感器放置在任一位置处，测量一次磁场，得到此磁场包括外部环境磁场和磁传感器初始固定偏移磁场 在同一位置处，磁传感器按照轴向旋转180度，再测量一次磁场，得到

步骤1B：利用和两个公式相加求出磁传感器轴向上的依次求出磁传感器不同轴的

步骤1C：在没有磁驱动机器人及驱动磁体的条件下，令磁传感器固定在某个位姿B，获得磁传感器读数

步骤1D：加入驱动磁体，在同一位姿B再测量一次，获得磁传感器读数

步骤1E：令获得进一步得到

5.如权利要求1所述的方法，其特征在于，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间所在的环境中指定位姿处的环境磁场；则步骤二在所述指定位姿处获得环境磁场。

6.如权利要求1所述的方法，其特征在于，磁驱动机器人所在运动空间任意测量点的环境磁场采用该测量点所在的子空间的环境磁场；所述子空间是通过对运动空间进行划分得到的；则步骤二在每个子空间的指定位姿处获得环境磁场。

7.如权利要求1所述的方法，其特征在于，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间内离散测量点拟合出的磁场函数曲线所求的环境磁场；则步骤二在多个位姿处获得环境磁场，通过拟合得到磁场函数曲线。

8.一种磁驱动机器人有效磁场测量装置，其特征在于，包括磁驱动机器人、驱动磁体、检测模块、驱动磁体运动引导模块和控制模块；检测模块内包含磁传感器及其信号采集处理部分；驱动磁体与磁传感器具有固定的相对位姿；所述磁驱动机器人通过与驱动磁体进行磁交互，获得期望的运动；

控制模块包括偏移测量控制子模块、环境磁场测量控制子模块、有效磁场测量控制子模块；

偏移测量控制子模块，配置用于控制磁传感器测得磁传感器初始固定偏移磁场和驱动磁体固定偏移磁场之和，即固定偏移

环境磁场测量控制子模块，配置用于通过驱动磁体运动引导模块将驱动磁体和测量模块中的磁传感器移动到磁驱动机器人工作环境中，在移除磁驱动机器人的情况下，控制磁传感器测量磁场数据，该数据减去所述固定偏移获得环境磁场

有效磁场测量控制子模块，配置用于在磁驱动机器人运动过程中，将磁传感器测得的磁场减掉固定偏移及环境磁场即为磁驱动机器人产生的有效磁场信号。

9.如权利要求8所述的装置，其特征在于，所述偏移测量控制子模块获得固定偏移的方式为：

在驱动磁体和磁传感器放置于屏蔽室的情况下，从磁传感器读取测量数据即为固定偏移

或者，在磁传感器放置于屏蔽箱的情况下，从磁传感器获取测量读数，该读数为磁传感器初始固定偏移磁场在去掉屏蔽箱、没有磁驱动机器人和驱动磁体的条件下，磁传感器固定在某个位姿A，从磁传感器获取测量读数在磁传感器和驱动磁体固定在所述位姿A的情况下再测一次，获得磁传感器读数令获得进一步得到

或者，在无屏蔽的正常工作环境中，磁传感器放置在任一位置处，控制磁传感器测量一次磁场，得到此磁场包括外部环境磁场和磁传感器初始固定偏移磁场 在同一位置处，通过驱动磁体运动引导模块控制磁传感器按照轴向旋转180度，再测量一次磁场，得到 利用和两个公式相加求出磁传感器轴向上的依次求出磁传感器不同轴的在移除磁驱动机器人及驱动磁体的情况下，令磁传感器固定在某个位姿B，获得磁传感器读数 在加入驱动磁体的情况下，在同一位姿B再测量一次，获得磁传感器读数 令获得进一步得到

10.如权利要求8所述的装置，其特征在于，环境磁场测量控制子模块在获取环境磁场时：

磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间所在的环境中指定位姿处的环境磁场；

或者，磁驱动机器人所在运动空间任意测量点的环境磁场采用该测量点所在的子空间的环境磁场；所述子空间是通过对运动空间进行划分得到的；

或者，磁驱动机器人所在运动空间任意测量点的环境磁场采用运动空间内离散测量点拟合出的磁场函数曲线所求的环境磁场。