本发明公开了一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统，包括内部嵌入永磁体的胶囊机器人、外部永磁体、机械臂、基于磁传感器阵列的定位装置。本发明的有益效果是：机械臂末端固定安装外部永磁体，实现灵活的空间磁场变化；胶囊机器人内部嵌入小体积的永磁体，在空间磁场的作用下进行翻转运动；该定位方法通过上、下层磁传感器阵列盘测得的空间磁场做差分处理，消除外部永磁体和地磁场的信息，实现胶囊机器人磁场强度的准确获取；动态调节磁传感器阵列盘的位置，获取胶囊机器人的磁场梯度信息，校验位姿计算结果，提高空间定位精度。
1.一种基于磁传感器阵列的磁控胶囊机器人定位系统，硬件设备包括磁控胶囊机器人(7)、外部永磁体(5)、机械臂(4)以及基于磁传感器阵列的定位装置；其特征在于：所述机械臂(4)的末端固定安装外部永磁体(5)，所述磁控胶囊机器人(7)内部嵌入小体积的永磁体；
所述基于磁传感器阵列的定位装置由上层磁传感器阵列盘(1)、下层磁传感器阵列盘(1’)、丝杠(2)以及底座(3)构成，所述上层磁传感器阵列盘(1)位于旋转病床(6)的上方，所述下层磁传感器阵列盘(1’)位于旋转病床(6)的下方，所述旋转病床(6)上平躺着体内装有磁控胶囊机器人(7)的病人，所述上层磁传感器阵列盘(1)和下层磁传感器阵列盘(1’)开设的滑槽(10)内固定安装有多组传感器(9)；
所述基于磁传感器阵列的磁控胶囊机器人定位系统的定位方法，包括以下步骤：
第一、建立空间坐标系，将所述外部永磁体(5)设置在原点坐标，分别调节上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)的位置，使两个磁传感器阵列盘以外部永磁体(5)运动的平面为中心上下对称分布；
第二、分别通过所述上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)测得初始时刻的空间磁场信息，该信息包括地磁场和外部永磁体(5)的磁场；
第三、调节所述外部永磁体(5)的位置控制磁控胶囊机器人(7)的运动，上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)实时检测空间所在点的磁场强度并发送给上位机处理器，此时，上层磁传感器阵列盘(1)检测的空间磁场强度包括磁控胶囊机器人(7)的磁场、外部永磁体(5)的磁场和地磁场；下层磁传感器阵列盘(1’)检测的空间磁场强度可近似包括外部永磁体(5)的磁场和地磁场；通过将上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)检测的空间磁场强度做差分处理，可消除外部永磁体(5)和地磁场的信息，得到磁控胶囊机器人(7)在空间某点的磁场强度值，利用第一公式实现胶囊机器人空间位姿的推导，所述第一公式包括：第四、控制所述外部永磁体(5)在水平或者竖直位置保持不变，同时调节丝杠(2)驱动上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)对称运动，并检测不同位置的磁场强度，通过上、下层磁传感器阵列盘在不同位置检测的磁场信息，利用第二公式可获取胶囊机器人的磁场梯度，进而对位姿计算结果校验，所述第二公式包括：其中，(x,y,z)为胶囊机器人在空间的位置参数，(x1,y1,z1)和(x2,y2,z2)为磁传感器的两个不同位置，G1和G2为对应两个位置的磁梯度张量矩阵，A1和A2为磁矩系数矩阵，E为单位矩阵。
2.根据权利要求1所述的一种基于磁传感器阵列的磁控胶囊机器人定位系统，其特征在于：所述丝杠(2)均匀分布在矩形底座的四个边角上，且丝杠(2)分为驱动上层磁传感器阵列盘的丝杠(201)和驱动下层磁传感器阵列盘的丝杠(202)两组，驱动上层磁传感器阵列盘的丝杠(201)穿过下层通孔(8)控制上层磁传感器阵列盘(1)运动，驱动下层磁传感器阵列盘的丝杠(202)穿过上层通孔(8)控制下层磁传感器阵列盘(1’)运动。
3.根据权利要求1所述的一种基于磁传感器阵列的磁控胶囊机器人定位系统，其特征在于：所述上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)之间的中心平面为外部永磁体(5)的运动平面，上层磁传感器阵列盘(1)与外部永磁体(5)之间的区域为磁控胶囊机器人(7)的运动空间。
一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统
技术领域
[0001] 本发明涉及一种磁控胶囊机器人定位系统，具体为一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统，属于三维定位技术领域。
背景技术
[0002] 磁控胶囊机器人能够安全、有效地对胃肠道疾病进行无创诊疗，避免传统胃肠镜诊断过程痛苦大、疗效差的缺陷，成为微型医疗机器人发展的重要里程碑。
[0003] 实时获取机器人在体内的位姿信息，是磁控胶囊机器人需要解决的首要问题。传统的超声成像、CT以及X光，通过穿透人体组织来实现设备的定位，检测精度虽然高，但是成本较高且会对人体造成一定伤害，不适合广泛和高频次的使用。另外，由于胃部环境的复杂性和实时变化特点，利用机器视觉来远程定位胶囊机器人仍具有较高的误差和局限性。通过磁传感器检测胶囊机器人的磁场信息，能够实时计算出该胶囊机器人的位姿变化情况。但是，由于外部强大的驱动磁场和地磁场的影响，利用磁传感器检测磁控胶囊机器人的磁场信息具有非常大的干扰，无法精确得到胶囊机器人的磁场变化信息。因此，有效地消除外部磁场和地磁场的干扰，开展高精度的磁控胶囊机器人实时定位系统成为目前研究的难点与热点。
[0004] 基于此，本申请提出一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统。
发明内容
[0005] 本发明的目的就在于为了解决问题而提供一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统。
[0006] 本发明通过以下技术方案来实现上述目的：一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统，包括硬件设备包括磁控胶囊机器人、外部永磁体、机械臂以及基于磁传感器阵列的定位装置；所述机械臂的末端固定安装外部永磁体，所述磁控胶囊机器人内部嵌入小体积的永磁体；
[0007] 所述基于磁传感器阵列的定位装置由上层磁传感器阵列盘、下层磁传感器阵列盘、丝杠以及底座构成，所述上层磁传感器阵列盘位于旋转病床的上方，所述下层磁传感器阵列盘位于旋转病床的下方，所述旋转病床上平躺着体内装有磁控胶囊机器人的病人，所述上层磁传感器阵列盘和下层磁传感器阵列盘开设的滑槽内固定安装有多组传感器。
[0008] 作为本发明再进一步的方案：所述丝杠均匀分布在矩形底座的四个边角上，且丝杠分为驱动上层磁传感器阵列盘的丝杠和驱动下层磁传感器阵列盘的丝杠两组，驱动上层磁传感器阵列盘的丝杠穿过下层通孔控制上层磁传感器阵列盘运动，驱动下层磁传感器阵列盘的丝杠穿过上层通孔控制下层磁传感器阵列盘运动。
[0009] 作为本发明再进一步的方案：所述上层磁传感器阵列盘与下层磁传感器阵列盘之间的中心平面为外部永磁体的运动平面，上层磁传感器阵列盘与外部永磁体之间的区域为磁控胶囊机器人的运动空间。
[0010] 一种基于磁传感器阵列的磁控胶囊机器人定位系统的定位方法，其定位方法包括以下步骤：
[0011] 第一、建立空间坐标系，将所述外部永磁体设置在原点坐标，分别调节上层磁传感器阵列盘与下层磁传感器阵列盘的位置，使两个磁传感器阵列盘以外部永磁体运动的平面为中心上下对称分布；
[0012] 第二、分别通过所述上层磁传感器阵列盘与下层磁传感器阵列盘测得初始时刻的空间磁场信息，该信息包括地磁场和外部永磁体的磁场；
[0013] 第三、调节所述外部永磁体的位置控制磁控胶囊机器人的运动，上层磁传感器阵列盘与下层磁传感器阵列盘实时检测空间所在点的磁场强度并发送给上位机处理器。此时，上层磁传感器阵列盘检测的空间磁场强度包括磁控胶囊机器人的磁场、外部永磁体的磁场和地磁场；下层磁传感器阵列盘检测的空间磁场强度可近似包括外部永磁体的磁场和地磁场；通过将上层磁传感器阵列盘与下层磁传感器阵列盘检测的空间磁场强度做差分处理，可消除外部永磁体和地磁场的信息，得到磁控胶囊机器人在空间某点的磁场强度值，利用第一公式实现胶囊机器人空间位姿的推导。所述第一公式包括：
[0014]
[0015]
[0016]
[0017] 第四、控制所述外部永磁体在水平或者竖直位置保持不变，同时调节丝杠驱动上层磁传感器阵列盘与下层磁传感器阵列盘对称运动，并检测不同位置的磁场强度。通过上、下层磁传感器阵列盘在不同位置检测的磁场信息，利用第二公式可获取胶囊机器人的磁场梯度，进而对位姿计算结果校验，所述第二公式包括：
[0018]
[0019] 其中，(x,y,z)为胶囊机器人在空间的位置参数，(x1,y1,z1)和(x2,y2,z2)为磁传感器的两个不同位置，G1和G2为对应两个位置的磁梯度张量矩阵，A1和A2为磁矩系数矩阵，E为单位矩阵。
[0020] 本发明的有益效果是：该基于磁传感器阵列的磁控胶囊机器人定位方法及系统设计合理，机械臂末端固定安装外部永磁体，实现灵活的空间磁场变化；胶囊机器人内部嵌入小体积的永磁体，在空间磁场的作用下进行翻转运动；定位装置主要包括上下两层的磁传感器阵列盘、丝杠、驱动电机和底座，丝杠同时控制两层磁传感器阵列盘的上下运动，驱动电机控制丝杠的运动。上、下层磁传感器阵列盘之间的中心平面为外部永磁体的运动平面，而上层磁传感器阵列盘与外部永磁体之间的区域为胶囊机器人的运动空间。该定位方法通过上、下层磁传感器阵列盘测得的空间磁场做差分处理，消除外部永磁体和地磁场的信息，实现胶囊机器人磁场强度的准确获取；动态调节磁传感器阵列盘的位置，获取胶囊机器人的磁场梯度信息，校验位姿计算结果，提高空间定位精度。
附图说明
[0021] 图1为本发明实施例中的定位系统示意图；
[0022] 图2为本发明实施例中的基于磁传感器阵列的定位装置；
[0023] 图3为本发明实施例中的定位方法流程图。
[0024] 图中：1、上层磁传感器阵列盘，1’、下层磁传感器阵列盘，2、丝杠，201、驱动上层磁传感器阵列盘的丝杠，202、驱动下层磁传感器阵列盘的丝杠，3、底座，4、机械臂，5、外部永磁体，6、旋转病床，7、磁控胶囊机器人，8、通孔，9、传感器和10、滑槽。
具体实施方式
[0025] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0026] 实施例一
[0027] 请参阅图1～2，一种基于磁传感器阵列的磁控胶囊机器人定位系统，包括硬件设备包括磁控胶囊机器人7、外部永磁体5、机械臂4以及基于磁传感器阵列的定位装置；所述机械臂4的末端固定安装外部永磁体5，所述磁控胶囊机器人7内部嵌入小体积的永磁体；
[0028] 所述基于磁传感器阵列的定位装置由上层磁传感器阵列盘1、下层磁传感器阵列盘1’、丝杠2以及底座3构成，所述上层磁传感器阵列盘1位于旋转病床6的上方，所述下层磁传感器阵列盘1’位于旋转病床6的下方，所述旋转病床6上平躺着体内装有磁控胶囊机器人7的病人，所述上层磁传感器阵列盘1和下层磁传感器阵列盘1’开设的滑槽10内固定安装有多组传感器9。
[0029] 在本发明实施例中，所述丝杠2均匀分布在矩形底座的四个边角上，且丝杠2分为驱动上层磁传感器阵列盘的丝杠201和驱动下层磁传感器阵列盘的丝杠202两组，驱动上层磁传感器阵列盘的丝杠201穿过下层通孔8控制上层磁传感器阵列盘1运动，驱动下层磁传感器阵列盘的丝杠202穿过上层通孔8控制下层磁传感器阵列盘1’运动。
[0030] 在本发明实施例中，所述上层磁传感器阵列盘1与下层磁传感器阵列盘1’之间的中心平面为外部永磁体5的运动平面，上层磁传感器阵列盘1与外部永磁体5之间的区域为磁控胶囊机器人7的运动空间。
[0031] 实施例二
[0032] 请参阅图3，一种基于磁传感器阵列的磁控胶囊机器人定位系统的定位方法，其定位方法包括以下步骤：
[0033] 第一、建立空间坐标系，将所述外部永磁体5设置在原点坐标，分别调节上层磁传感器阵列盘1与下层磁传感器阵列盘1’的位置，使两个磁传感器阵列盘以外部永磁体5运动的平面为中心上下对称分布；
[0034] 第二、分别通过所述上层磁传感器阵列盘1与下层磁传感器阵列盘1’测得初始时刻的空间磁场信息，该信息包括地磁场和外部永磁体5的磁场；
[0035] 第三、调节所述外部永磁体5的位置控制磁控胶囊机器人7的运动，上层磁传感器阵列盘1与下层磁传感器阵列盘1’实时检测空间所在点的磁场强度并发送给上位机处理器。此时，上层磁传感器阵列盘1检测的空间磁场强度包括磁控胶囊机器人7的磁场、外部永磁体5的磁场和地磁场；下层磁传感器阵列盘1’检测的空间磁场强度可近似包括外部永磁体5的磁场和地磁场；通过将上层磁传感器阵列盘1与下层磁传感器阵列盘1’检测的空间磁场强度做差分处理，可消除外部永磁体5和地磁场的信息，得到磁控胶囊机器人7在空间某点的磁场强度值，利用第一公式实现胶囊机器人空间位姿的推导。所述第一公式包括：
[0036]
[0037]
[0038]
[0039] 第四、控制所述外部永磁体5在水平或者竖直位置保持不变，同时调节丝杠2驱动上层磁传感器阵列盘1与下层磁传感器阵列盘1’对称运动，并检测不同位置的磁场强度。通过上、下层磁传感器阵列盘在不同位置检测的磁场信息，利用第二公式可获取胶囊机器人的磁场梯度，进而对位姿计算结果校验，所述第二公式包括：
[0040]
[0041] 其中，(x,y,z)为胶囊机器人在空间的位置参数，(x1,y1,z1)和(x2,y2,z2)为磁传感器的两个不同位置，G1和G2为对应两个位置的磁梯度张量矩阵，A1和A2为磁矩系数矩阵，E为单位矩阵。
[0042] 在使用该基于磁传感器阵列的磁控胶囊机器人定位方法及系统时，通过上、下层磁传感器阵列盘测得的空间磁场做差分处理，消除外部永磁体和地磁场的信息，实现胶囊机器人磁场强度的准确获取；动态调节磁传感器阵列盘的位置，获取胶囊机器人的磁场梯度信息，校验位姿计算结果，提高空间定位精度。
[0043] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
[0044] 此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
