[发明专利]一种基于磁传感器阵列的磁控胶囊机器人定位方法及系统

权利要求书

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)为胶囊机器人在空间的位置参数，(x₁,y₁,z₁)和(x₂,y₂,z₂)为磁传感器的两个不同位置，G₁和G₂为对应两个位置的磁梯度张量矩阵，A₁和A₂为磁矩系数矩阵，E为单位矩阵。

2.根据权利要求1所述的一种基于磁传感器阵列的磁控胶囊机器人定位系统，其特征在于：所述丝杠(2)均匀分布在矩形底座的四个边角上，且丝杠(2)分为驱动上层磁传感器阵列盘的丝杠(201)和驱动下层磁传感器阵列盘的丝杠(202)两组，驱动上层磁传感器阵列盘的丝杠(201)穿过下层通孔(8)控制上层磁传感器阵列盘(1)运动，驱动下层磁传感器阵列盘的丝杠(202)穿过上层通孔(8)控制下层磁传感器阵列盘(1’)运动。

3.根据权利要求1所述的一种基于磁传感器阵列的磁控胶囊机器人定位系统，其特征在于：所述上层磁传感器阵列盘(1)与下层磁传感器阵列盘(1’)之间的中心平面为外部永磁体(5)的运动平面，上层磁传感器阵列盘(1)与外部永磁体(5)之间的区域为磁控胶囊机器人(7)的运动空间。