[发明专利]胶囊内窥镜系统及其胶囊内窥镜磁定位方法

权利要求书

1.一种胶囊内窥镜系统，其特征在于，

包括胶囊内窥镜、磁控装置以及处理装置，其中

所述胶囊内窥镜具有内置空间以及布置在所述内置空间中的第一磁体、惯性传感器和至少两个磁传感器，所述至少两个磁传感器中的两个磁传感器分别靠近所述内置空间沿纵向的两个端部；

所述磁控装置具有第二磁体，所述磁控装置通过所述第二磁体对所述第一磁体的作用以对所述胶囊内窥镜进行磁控制；

所述惯性传感器感应于所述胶囊内窥镜的运动姿态并获取姿态测量值，所述至少两个磁传感器感应于所述第二磁体的磁场并至少获取第一磁场测量值和第二磁场测量值；

所述处理装置，基于预设姿态解算算法和所述姿态测量值计算所述胶囊内窥镜的自旋角度，所述自旋角度包括精估俯仰角、精估翻滚角和预估偏航角；所述处理装置基于驱动磁场算法以及所述第一磁场测量值、所述第二磁场测量值和所述自旋角度，计算所述胶囊内窥镜相对于第二磁体的预估位置，并使用无迹卡尔曼滤波算法对所述预估位置和所述预估偏航角进行优化，获得所述胶囊内窥镜相对于所述第二磁体的精估位置和精估偏航角，基于所述第二磁体的位置和姿态，确定所述胶囊内窥镜在世界坐标系中的位置和姿态。

2.根据权利要求1所述的胶囊内窥镜系统，其特征在于，

所述磁传感器为三轴测量、数字输出的微机电系统器件。

3.根据权利要求2所述的胶囊内窥镜系统，其特征在于，

所述至少两个磁传感器中更加靠近所述第一磁体放置的磁传感器为霍尔传感器。

4.根据权利要求2所述的胶囊内窥镜系统，其特征在于，

所述至少两个磁传感器中更加远离所述第一磁体放置的磁传感器为各向异性磁电阻传感器。

5.根据权利要求1所述的胶囊内窥镜系统，其特征在于，

所述惯性传感器为六轴惯性传感器。

6.根据权利要求5所述的胶囊内窥镜系统，其特征在于，

所述惯性传感器包括加速度计和陀螺仪。

7.根据权利要求1所述的胶囊内窥镜系统，其特征在于，

所述磁控装置还具有与所述第二磁体配合以对所述胶囊内窥镜进行磁控制的线圈，并且当所述磁控装置对所述胶囊内窥镜进行磁定位时，所述线圈处于非工作状态。

8.根据权利要求7所述的胶囊内窥镜系统，其特征在于，

所述惯性传感器和所述至少两个磁传感器具有大致相同的坐标朝向。

9.根据权利要求1所述的胶囊内窥镜系统，其特征在于，

所述驱动磁场算法将所述第二磁体等效为磁偶极矩。

10.一种胶囊内窥镜系统的胶囊内窥镜磁定位方法，其特征在于，包括：

获取胶囊内窥镜的姿态测量值；

在所述胶囊内窥镜的第一位置和第二位置分别感应第二磁体的磁场并获取第一磁场测量值和第二磁场测量值，其中，第一位置和第二位置分别靠近所述胶囊内窥镜的内置空间沿纵向的两个端部，所述第二磁体用于对所述胶囊内窥镜进行磁控制；

基于预设姿态解算算法和所述姿态测量值计算所述胶囊内窥镜的自旋角度，所述自旋角度包括精估俯仰角、精估翻滚角和预估偏航角；基于驱动磁场算法以及所述第一磁场测量值、所述第二磁场测量值和所述自旋角度，计算所述胶囊内窥镜相对于第二磁体的预估位置，并使用无迹卡尔曼滤波算法对所述预估位置和所述预估偏航角进行优化，获得所述胶囊内窥镜相对于所述第二磁体的精估位置和精估偏航角，基于所述第二磁体的位置和姿态，确定所述胶囊内窥镜在世界坐标系中的位置和姿态。