无线胶囊内窥镜的定位系统、装置及计算机设备的制作方法

1.本技术属于医疗设备技术领域，特别是涉及一种无线胶囊内窥镜的定位系统、装置及计算机设备。


背景技术：

2.无线胶囊内窥镜技术是一种用于完整的消化道检查的技术，具有无痛苦、非侵入性等特点。无线胶囊内窥镜实际上是一颗形如胶囊的微型机器人，它搭载有照明模块、相机模块、图像处理模块和无线传输模块等。无线胶囊内窥镜被病人吞咽进入人体消化道后，可以在体内拍摄图像并实时传输到体外。医生或计算机可以根据接收到的图像进行疾病诊断。
3.在使用无线胶囊内窥镜进行消化道检查的过程中，实时、精确地对无线胶囊内窥镜进行定位，关系着最终诊断结果的正确性。通常，由无线胶囊内窥镜的三维位置和三维旋转组成的六维位姿可以用以描述无线胶囊内窥镜在消化道中的精确位置。但是，现有技术中对于无线胶囊内窥镜的实时定位往往最多只能求解其五维位姿。例如，基于磁驱动的无线胶囊内窥镜的定位技术，都是使用体外永磁铁驱动一个内嵌永磁铁的无线胶囊内窥镜，然后通过磁传感器测量体外永磁铁和胶囊内部永磁铁的叠加磁场，进而计算得到无线胶囊内窥镜的实时五维位姿，该方法不能确定无线胶囊内窥镜绕内部永磁铁磁矩方向的旋转角度。一些研究者通过采用一段时间内测得的五维位姿序列估算无线胶囊内窥镜的前进方向，从而得到其六维位姿，但这种方法无法直观地得到无线胶囊内窥镜在每个时刻相对于世界坐标系的三维旋转，且由于需要一段时间内的测量值才能估计第六维位姿，无线胶囊内窥镜的定位频率被大大降低，容易对消化道检查带来不利影响。因此，在使用无线胶囊内窥镜进行消化道检查的过程中，如何实时、精确地估计无线胶囊内窥镜的六维位姿便是本领域技术人员亟需解决的一个难题。


技术实现要素：

4.有鉴于此，本技术提供了一种无线胶囊内窥镜的定位系统、装置及计算机设备，通过采用基于永磁铁和电磁线圈的复合式体外驱动器，并融合胶囊内的多个磁传感器，可以利用胶囊内磁传感器和惯性传感器提供的数据实时估计胶囊的六维位姿，有助于在较大的立体工作空间中实现高精度、高更新速率的定位。
5.本技术的第一方面提供了一种无线胶囊内窥镜的定位系统，所述系统包括无线胶囊内窥镜、体外驱动器以及分别与所述无线胶囊内窥镜和所述体外驱动器通信连接的计算机设备；其中，所述体外驱动器中内置有多个体外磁源，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；所述无线胶囊内窥镜中内置有胶囊永磁铁、惯性传感器以及多个磁传感器；所述计算机设备用于执行如下操作：
6.在控制所述体外驱动器驱动所述无线胶囊内窥镜运动的过程中，获取所述惯性传
感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及每个所述磁传感器测量到的测量磁场值；
7.确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算多个所述体外磁源在每个所述磁传感器处的理论磁场值；
8.对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
9.基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
10.本技术的第二方面提供了一种无线胶囊内窥镜的定位装置，包括：
11.获取模块，用于在控制体外驱动器驱动所述无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
12.计算模块，用于确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
13.分离模块，用于对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
14.定位模块，用于基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
15.本技术的第三方面提供了一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如下操作：
16.在控制体外驱动器驱动无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
17.确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
18.对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
19.基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
20.本技术的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如下操作：
21.在控制体外驱动器驱动无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
22.确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
23.对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
24.基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
25.本技术的第五方面提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如下操作：
26.在控制体外驱动器驱动无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
27.确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
28.对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
29.基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
30.与现有技术相比，本技术具有以下优点：
31.本技术提供的定位系统使用基于永磁铁-电磁线圈的复合式体外驱动器，并结合胶囊内置的多个磁传感器，用于对磁驱动下的无线胶囊内窥镜的实时六自由度定位。本技术提供的定位系统仅在胶囊内部采用一个微型低功耗的惯性传感器和三个以特殊排布方式安装的磁传感器，不需要在体外设置传感器即可进行胶囊的实时六维定位，且用电量低于一般的内部定位机构，减小了胶囊的体积和重量，提高了电池利用率。工作空间可扩大到机械臂的整个工作空间，不受体外传感器安装位置的限制。
32.其次，本技术提供的定位系统中，体外驱动器采用两个正交的电磁线圈套着一个永磁铁的安装方式，结构简单且易于安装，相较于其他使用外部磁传感器的方案更加灵活，有助于提供更大的工作空间，而不要求被检查者置身于一个固定的传感器阵列结构中，提高了定位的准确性和稳定性。
33.第三，本技术提供的定位系统中，由机械臂某端持一个球形的体外驱动器，通过改变体外驱动器内永磁铁磁场来控制胶囊在体内的运动，装置简单且易于安装，相较于仅依赖电磁线圈的驱动装置更加小巧轻便，可提供更大的工作空间，体外驱动器的球形表面更适合与病人体表接触、降低安全风险。
34.第四，本技术通过对体外驱动器内永磁铁和电磁线圈的磁场及胶囊内被动永磁铁的磁场建立数学模型，并基于体外磁源产生磁场控制胶囊，使用胶囊内置的磁传感器感知磁场，使用胶囊内置的惯性传感器感知胶囊旋转，并使用信号处理算法和非线性优化算法
对胶囊进行实时的六维位姿定位。该定位算法不受驱动磁场影响，定位结果为胶囊相对于固定世界坐标系的六维位姿，更加直观和便于使用，并可以实现更高的定位频率。
附图说明
35.为了更清楚地说明本技术实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
36.图1是本技术实施例提供的一种无线胶囊内窥镜的定位系统的示意图；
37.图2是本技术实施例提供的一种无线胶囊内窥镜的内部结构示意图；
38.图3是本技术实施例提供的一种无线胶囊内窥中内置的多个磁传感器的排布方式示意图；
39.图4是本技术实施例提供的一种体外驱动器的示意图；
40.图5是本技术实施例提供的一种无线胶囊内窥镜的定位算法的示意图；
41.图6是本技术实施例提供的一种无线胶囊内窥镜中内置的磁传感器测量体外磁源产生的磁场的示意图；
42.图7是本技术实施例提供的一种测量磁场值分离算法的示意图；
43.图8是本技术实施例提供的一种无线胶囊内窥镜的定位装置的示意图；
44.图9是本技术实施例提供的一种计算机设备的示意图。
具体实施方式
45.以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本技术实施例。然而，本领域技术人员应当清楚，在没有这些具体细节的其他实施例中也可以实现本技术。在其他情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本技术的描述。
46.下面通过具体实施例来说明本技术的技术方案。
47.参照图1，示出了本技术实施例提供的一种无线胶囊内窥镜的定位系统的示意图，该定位系统可以包括检查床、机械臂、无线胶囊内窥镜、体外驱动器以及计算机设备。其中：
48.(1)检查床
49.检查床可供病人平躺在其上。
50.(2)机械臂
51.机械臂可以位于检查床附近。机械臂包括一末端执行器，该末端执行器可用于安装体外驱动器。机械臂的末端执行器与体外驱动器之间可以采用刚性连接。例如，可以通过3d打印的方式将上述结构刚性连接。在本技术实施例中，如图1所示，可以基于机械臂底座中心定义一世界坐标系{w}。
52.(3)无线胶囊内窥镜
53.无线胶囊内窥镜在被病人吞咽进入体内后，可以被体外驱动器驱动，从而在消化道等腔道内运动并采集体内环境的图像供计算机设备或医生诊断。如图2所示，是本技术实施例提供的一种无线胶囊内窥镜的内部结构示意图。图2所示的无线胶囊内窥镜中除了传
统的相机模块、照明模块、无线通信模块、微处理模块、纽扣电池以外，还内置有胶囊永磁铁、惯性传感器(imu)以及多个磁传感器。其中，胶囊永磁铁可以是圆柱形永磁铁，该胶囊永磁铁可以沿胶囊自身轴线方向充磁并与惯性传感器的z轴方向一致。惯性传感器可用于对人体内胶囊相对于固定的世界坐标系的二自由度旋转进行估计，得到二轴旋转数据(α,β)。图2中所示的磁传感器的数量共三个，这三个磁传感器可以以特定排布方式安装于无线胶囊内窥镜中，用于对胶囊在人体内的四自由度位姿估计(胶囊的三维位置和方向角γ)。基于图2中的无线胶囊内窥镜可以构建一胶囊坐标系{c}。
54.结合图1可知，本技术实施例中定义的世界坐标系{w}为机械臂的底座中心。由于体外驱动器相对于固定的世界坐标系{w}的位姿可以很容易地通过机械臂运动学模型被实时准确地测得，因此只要获取无线胶囊内窥镜相对于体外驱动器的位姿，即可计算得到无线胶囊内窥镜相对于上述世界坐标系{w}的位姿。基于上述分析，本技术实施例通过在无线胶囊内窥镜中安装磁传感器，由磁传感器来测量体外磁源在胶囊所处位置产生的磁场，从而求解体内无线胶囊内窥镜相对于体外驱动器的位姿。
55.结合图1和图2可知，无线胶囊内窥镜中内置有胶囊永磁铁用于胶囊的驱动功能。在检查过程中，该胶囊永磁铁也会产生磁场并被胶囊内的磁传感器感知到。为了使用胶囊内的磁传感器准确地测量体外磁场产生的磁场，本技术实施例针对胶囊内各个磁传感器的排布方式进行了具体的设计，可以使得胶囊永磁铁产生的磁场不会影响磁传感器对体外磁源产生的磁场的测量。即，本技术实施例通过对胶囊内多个磁传感器的排布方式进行设计，可以使得胶囊永磁铁产生的磁场在以特定排布方式安装的多个磁传感器上的分量为零。
56.如图3所示，是本技术实施例提供的一种无线胶囊内窥中内置的多个磁传感器的排布方式示意图，图3中同时还示出了圆柱形的胶囊永磁铁产生的磁场的示例。安装于胶囊内的磁传感器可以包括第一磁传感器、第二磁传感器和第三磁传感器。如图3所示，在胶囊坐标系{c}的xy平面上的任意位置，胶囊永磁铁产生的磁场向量均垂直于xy平面。因此，可以在胶囊坐标系{c}的x轴上确定一点，垂直于x轴方向安装一个磁传感器，则该磁传感器测得的胶囊永磁铁的磁场分量b
cx
＝0。即，第一磁传感器可以安装于图3所示的胶囊坐标系{c}的x轴上一点a且垂直于胶囊坐标系的x轴方向。同理，可以在胶囊坐标系{c}的y轴上确定一点，垂直于y轴方向安装一个磁传感器，该磁传感器测得的胶囊永磁铁的磁场分量b
cy
＝0。即，第二磁传感器可以安装于图3所示的胶囊坐标系{c}的y轴上一点b且垂直于胶囊坐标系的y轴方向。在磁感线切线斜率为0的位置，例如图3中的c点，胶囊永磁铁产生的磁场向量仅包含沿xy平面的分量，而沿z轴分量为零。因此，可以在点c处垂直于z轴方向安装一个磁传感器，则该磁传感器测得的胶囊永磁铁的磁场分量b
cz
＝0。即，第三磁传感器可以安装于图3所示的胶囊永磁铁产生的磁感线切线斜率为0的一点c且垂直于胶囊坐标系{c}的z轴方向。
57.通过在无线胶囊内窥镜中内置的胶囊永磁铁周围的a、b、c三点以上述特定排布方式安装三个磁传感器，可以分别测得体外磁源产生的磁场在胶囊坐标系{c}的x、y、z轴方向上的分量，而不会受到胶囊永磁铁产生的磁场的影响。
58.在本技术实施例中，假设胶囊坐标系{c}下的三个磁传感器的位置和法向量分别为则无线胶囊内窥镜在世界坐标系{w}下的六维位姿可以由三维位置和三维旋转矩阵rc∈so(3)表示。那么，第i个磁传感器在世界坐标系{w}下的位置
和法向量方向可以分别表示为：和
59.(4)体外驱动器
60.体外驱动器可以安装于机械臂的末端执行器并处于检查床的上方。如图4所示，是本技术实施例提供的一种体外驱动器的示意图。体外驱动器可以包括多个体外磁源，如图4中的(a)所示的体外永磁铁、第一电磁线圈和第二电磁线圈。其中，第一电磁线圈和第二电磁线圈的轴线正交。在对第一电磁线圈和第二电磁线圈通电后，第一电磁线圈和第二电磁线圈可以分别产生相应的电磁场。体外永磁铁可以为圆柱形永磁铁，体外永磁铁可以沿自身轴向方向充磁并与机械臂末端关节旋转轴方向正交。如图4中的(a)所示，体外永磁铁可以嵌套于第一电磁线圈和第二电磁线圈内。基于体外驱动器可以形成一驱动器坐标系{a}，体外永磁铁、第一电磁线圈和第二电磁线圈的单位磁矩可以分别与驱动器坐标系{a}的各轴方向相平行。例如，在图4中的(a)中，还示出了各个体外磁源的单位磁矩与驱动器坐标系{a}的各轴方向在不同方向的视图中的示例。其中，体外永磁铁的单位磁矩与驱动器坐标系{a}的z轴平行，第一电磁线圈的单位磁矩与驱动器坐标系{a}的y轴平行，第二电磁线圈的单位磁矩与驱动器坐标系{a}的x轴平行。这样，体外永磁铁、第一电磁线圈和第二电磁线圈的单位磁矩两两正交。在本技术实施例中，体外驱动器的外壳形状可以为球形，其外壳材质可以为塑料材质。这样，在控制体外驱动器的过程中，能够降低检查时因体外驱动器接触病人身体而对病人带来额外伤害的风险。
61.如图4中的(b)所示，是图4中的(a)所示的体外磁源产生的磁场的示意图。由于单个磁源产生的磁场绕自身磁矩方向呈圆柱对称性，因此使用单个磁源产生的磁场的测量值进行胶囊定位时可能出现多解的情况。例如，假设胶囊的真实位置如图4中的(b)所示，但单个磁源的磁矩方向与世界坐标系{w}的z轴平行时，使用测量得到的磁场值对胶囊进行四自由度定位(胶囊的三维位置和方向角γ)，将会产生绕该磁源的磁矩方向呈圆柱对称的无数多个解，如图4中的(c)所示的圆圈上的任意一点均可以被认为是求解出的胶囊的位置。当使用两个磁矩方向正交的磁源(如)进行定位时，由于圆柱对称性被破坏，可以得到关于镜像对称的两个解，如图4中的(c)中所示的圆圈上的两个圆点a和b，上述两个圆点a和b所对应的位置可以被认为是求解出的胶囊的位置。而使用三个磁矩正交的磁源(如)进行定位时，由于三个具有圆柱对称性的磁场进行叠加后不再对称，根据三个磁源独立的测量磁场值，可以得到胶囊四自由度位姿的唯一解，如图4中的(c)中所示的圆圈上的圆点b，该圆点b所对应的位置可以被认为是求解出的胶囊的位置。通过使用三个磁矩两两正交的磁源进行定位，在理论上可以获得稳定、准确且唯一的定位结果。因此，本技术实施例采用三个体外磁源设计体外驱动器。
62.(5)计算机设备
63.计算机设备可以是位于检查床附近的一台通用计算机，该计算机设备中可以存储有与磁驱动及定位相关的程序和算法。在检查时，该计算机设备可以由工作人员或医生操作，从而对机械臂发出控制指令，采集机械臂以及体外驱动器的位置数据、无线胶囊内窥镜中各个磁传感器测量到的磁场数据以及惯性传感器测量到的姿态数据。计算机设备可以根据采集到的数据，运行定位算法，计算并可视化体内胶囊的实时六维位姿。
64.参见图5，示出了本技术实施例提供的一种无线胶囊内窥镜的定位算法的示意图，图1中所示的计算机设备通过执行图5所示的算法流程，可以实时输出无线胶囊内窥镜准确的六维位姿。具体地，在本技术实施例中，计算机设备可以通过胶囊内置的惯性传感器实时测量的方向角数据和机械臂反馈的末端连接的体外驱动器的位置数据，使用磁偶极子模型计算体外驱动器内三个体外磁源在每个磁传感器上的理论磁场值。同时，计算机设备可以通过一段时间内电磁线圈上电流的采样数据和胶囊内磁传感器测量得到的磁场数据(测量磁场值)，将体外驱动器内三个体外磁源的测量磁场值进行分离提取，并通过非线性优化拟合每个磁源的理论磁场值与测量磁场值，求解出胶囊位姿中的未知参数，从而实现胶囊在人体内被驱动的同时获取其实时六维位姿估计。
65.下面，结合图5所示的算法流程，对计算机设备执行该算法流程实时输出无线胶囊内窥镜准确的六维位姿的过程进行详细介绍。
66.如图5所示，计算机设备在控制体外驱动器驱动无线胶囊内窥镜运动的过程中，可以首先获取惯性传感器测量到的无线胶囊内窥镜的二轴旋转数据。然后，计算机设备可以确定体外驱动器的位置数据，并根据上述二轴旋转数据和位置数据计算多个体外磁源在每个磁传感器处的理论磁场值。其中，体外驱动器的位置数据可以是指体外驱动器在预设的世界坐标系{w}下的位置数据。
67.在具体实现中，计算机设备可以分别确定每个磁传感器在基于无线胶囊内窥镜形成的胶囊坐标系{c}下的位置和法向量方向然后根据二轴旋转数据和每个磁传感器在胶囊坐标系{c}下的位置和法向量方向确定每个磁传感器在世界坐标系{w}下的位置和法向量方向。
68.结合图1，无线胶囊内窥镜在世界坐标系{w}下的六维位姿可以由三维位置和三维旋转矩阵rc∈so(3)表示。其中，rc可以由胶囊坐标系{c}依次绕着世界坐标系{w}的x、y、z轴旋转的角度表示为：
69.rc＝rot(z，γ)rot(y，β)rot(x，α)
70.胶囊坐标系{c}依次绕着世界坐标系{w}的x、y轴旋转的角度(α，β)也就是无线胶囊内窥镜的二轴旋转数据，其可以通过胶囊内置的惯性传感器实时精确地测量得到。因此，在求解胶囊六维位姿的过程中，未知量为三维位置pc和方向角γ，共四个未知参数。
71.由于已知尺寸的体外驱动器与机械臂的末端执行器刚性连接，且可以通过机械臂已知的运动学模型实时估算机械臂的末端位姿，因此可以认为体外驱动器的六维位姿(pa，ra)能够被实时准确地测量到。假设体外驱动器中内置的体外永磁铁、第一电磁线圈和第二电磁线圈的中心位置均位于处，上述三个体外磁源的磁矩方向分别与驱动器坐标系{a}的z、y、x轴重合，则这三个体外磁源的单位磁矩方向可以表示为：
[0072][0073]
根据前述介绍，胶囊内第i个磁传感器在世界坐标系{w}下的位置可以表示为法向量方向可以表示为计算机设备可以基于上述每个磁
传感器在世界坐标系{w}下的位置和法向量方向计算每个体外磁源在每个磁传感器处的理论磁场值。
[0074]
根据磁偶极子模型，体外驱动器中的三个体外磁源在胶囊内第i个磁传感器上的理论磁场值b
i，a1
，b
i，a2
，b
i，a3
可以通过磁场在si方向上的投影求得。上述三个体外磁源在三个磁传感器上的理论磁场值可以分别表示为向量b
a1
(pc，γ)，b
a2
(pc，γ)，b
a3
(pc，γ)：
[0075][0076]
即，计算机设备可以基于磁偶极子模型和每个磁传感器在世界坐标系{w}下的位置和法向量方向，计算每个体外磁源产生的磁场在每个磁传感器在世界坐标系{w}下的法向量方向上的投影，得到每个体外磁源在每个磁传感器处的理论磁场值。
[0077]
另一方面，在控制体外驱动器驱动无线胶囊内窥镜运动的过程中，计算机设备还可以获取到胶囊内置的每个磁传感器测量到的测量磁场值。
[0078]
如图6所示，是本技术实施例提供的一种无线胶囊内窥镜中内置的磁传感器测量体外磁源产生的磁场的示意图。由于体外驱动器中包含体外永磁铁、第一电磁线圈和第二电磁线圈共三个体外磁源，胶囊内每个磁传感器测量到的磁场数据(测量磁场值)都是这三个体外磁源产生的磁场的叠加结果。例如，体外永磁铁、第一电磁线圈和第二电磁线圈产生的磁场在胶囊内置的某一磁传感器处的大小分别为b
a1
、b
a2
、b
a3
，则该磁传感器测量得到的磁场应为上述三个体外磁源产生的磁场的叠加结果，即：b＝b
a1
+b
a2
+b
a3
。因此，需要从磁传感器获取到的合成磁场中准确地检测和分离提取三个体外磁源的磁场值，即需要对每个磁传感器测量到的测量磁场值进行分离，得到每个体外磁源在每个磁传感器处的分离磁场值，以用于后续的定位算法。
[0079]
如图5所示，计算机设备对胶囊内每个磁传感器测量到的测量磁场值进行分离，可以结合电磁线圈上的电流测量结果来实现。
[0080]
在具体实现中，计算机设备首先可以控制向第一电磁线圈和第二电磁线圈分别通以不同频率的周期方波电流。例如，计算机设备可以控制向第一电磁线圈通以第一频率f1的周期方波电流i1，使得第一电磁线圈产生第一频率f1的周期时变磁场；同时，计算机设备可以控制向第二电磁线圈通以第二频率f2的周期方波电流i2，使得第二电磁线圈产生第二频率f2的周期时变磁场。
[0081]
如图7所示，是本技术实施例提供的一种测量磁场值分离算法的示意图。按照图7所示的算法，针对第一电磁线圈和第二电磁线圈上的电流，以及每个磁传感器上的测量磁场值，计算机设备可以对其进行采样，得到电流采样结果以及测量磁场值采样结果。针对电流采样结果，在进行快速傅里叶变换后，可以得到对应于第一频率f1以及第二频率f2的电流相位估计。
[0082]
由于在极短的采样时间内体外驱动器可以近似地认为处于禁止状态，体外永磁铁产生的磁场可以被视为恒定磁场，因此如图7所示，可以通过对测量磁场值采样结果计算时间平均值，并将该时间平均值作为体外永磁铁的估计值，也就是体外永磁铁产生的磁场在
对应的磁传感器处的分离磁场值。在使用每个磁传感器的测量磁场值采样结果减去上述体外永磁铁的估计值后，即可得到第一电磁线圈和第二电磁线圈的测量磁场值之和的时间序列。
[0083]
为了进一步分离第一电磁线圈和第二电磁线圈的测量磁场值，计算机设备可以对上述第一电磁线圈和第二电磁线圈的测量磁场值之和的时间序列进行快速傅里叶变换，可以得到对应于第一频率f1以及第二频率f2的相位估计和幅值估计。根据电流采样结果得到的电流相位估计以及上述第一电磁线圈和第二电磁线圈的测量磁场值之和的时间序列得到的相位估计，可以确定第一电磁线圈和第二电磁线圈的测量磁场值的符号。通过将第一电磁线圈和第二电磁线圈的幅值估计与测量磁场值的符号分别相乘，即可得到第一电磁线圈和第二电磁线圈分别在每个磁传感器上的分离磁场值。至此，体外驱动器内置的三个体外磁源在三个磁传感器上的测量磁场值便被分离出来了。
[0084]
下面，基于图7所示的算法，对上述针对测量磁场值的分离过程进行详细介绍。
[0085]
首先，结合图6，计算机设备可以控制向第一电磁线圈通以第一频率f1的周期方波电流i1，向第二电磁线圈通以第二频率f2的周期方波电流i2。然后，计算机设备可以分别对第一电磁线圈和第二电磁线圈上的电流进行采样，得到第一电磁线圈和第二电磁线圈的电流采样结果；通过分别对上述电流采样结果进行快速傅里叶变换，可以得到分别与第一频率f1和第二频率f2对应的电流相位估计，该电流相位估计包括第一电磁线圈的第一电流相位估计和第二电磁线圈的第二电流相位估计
[0086]
对于测量磁场值，计算机设备可以按照预设采样频率f对预设时间段t内每个磁传感器测量到的测量磁场值bi，i∈{1，2，3}进行采样，分别得到与每个磁传感器对应的测量磁场值采样序列。其中，预设采样频率f满足如下条件：f＞5f1，f＞5f2；预设时间段t满足如下条件：t＞1/f1，t＞1/f2。
[0087]
计算机设备可以分别计算每个测量磁场值采样序列的时间平均值：
[0088][0089]
上述时间平均值可以作为体外永磁铁在对应的磁传感器处的分离磁场值。
[0090]
在分别采用每个测量磁场值采样序列减去对应的磁传感器的时间平均值后，可以得到第一电磁线圈和第二电磁线圈产生磁场在每个磁传感器处的测量磁场值之和的时间序列：计算机设备基于上述测量磁场值之和的时间序列对第一电磁线圈和第二电磁线圈产生的测量磁场值进行分离，得到第一电磁线圈和第二电磁线圈在每个磁传感器处的分离磁场值。
[0091]
具体地，可以对上述测量磁场值之和的时间序列进行快速傅里叶变换，得到相位估计和幅值估计，该相位估计包括第一电磁线圈的第一相位估计和第二电磁线圈的第二相位估计上述幅值估计包括第一电磁线圈的第一幅值估计||b
′
i，a2
||和第二电磁线圈的第二幅值估计||b
′
i，a3
||。计算机设备可以根据计算得到的上述电流
相位估计、相位估计和幅值估计，计算第一电磁线圈和第二电磁线圈在每个磁传感器处的分离磁场值。
[0092]
其中，可以根据电流相位估计和相位估计，分别计算第一电磁线圈和第二电磁线圈的相位差的绝对值。例如，第一电磁线圈的相位差的绝对值可以表示为：第二电磁线圈的相位差的绝对值可以表示为：根据相位差的绝对值，可以确定第一电磁线圈和第二电磁线圈的测量磁场值符号。具体地，若相位差的绝对值在0～0.5
π
之间或1.5π～2π之间，则可以确定该线圈的测量磁场值符号为正；若相位差的绝对值在0.5
π
～1.5
π
之间，则该线圈的测量磁场值符号为负。即：若第一电磁线圈的相位差的绝对值介于0-0.5π或1.5π-2ππ之间，则可以确定第一电磁线圈的测量磁场值符号为正；若第一电磁线圈的相位差的绝对值介于0.5π-1.5π之间，则可以确定第一电磁线圈的测量磁场值符号为负；同理，若第二电磁线圈的相位差的绝对值介于0-0.5π或1.5π-2π之间，则可以确定第二电磁线圈的测量磁场值符号为正；若第二电磁线圈的相位差的绝对值介于0.5π-1.5π之间，则可以确定第二电磁线圈的测量磁场值符号为负。
[0093]
通过将测量磁场值符号分别与对应的幅值估计相乘，可以得到第一电磁线圈和第二电磁线圈在每个磁传感器处的分离磁场值。这样，体外驱动器内置的三个体外磁源在三个磁传感器上的测量磁场值便被分离为：
[0094][0095]
在完成对多个体外磁源的测量磁场值的分离，得到每个体外磁源在每个磁传感器处的分离磁场值后，如图5所示，可以基于理论磁场值和分离磁场值对无线胶囊内窥镜进行定位，优化求解胶囊位姿的未知参数，输出无线胶囊内窥镜的实时六维位姿。
[0096]
在本技术实施例中，可以基于理论磁场值和分离磁场值，采用最小二乘法求解无线胶囊内窥镜的六维位姿中的未知参数。
[0097]
如前所述，无线胶囊内窥镜的实时六维位姿由三维位置和三维旋转矩阵rc∈so(3)组成，其中三维旋转矩阵rc∈so(3)中的二轴旋转数据为基于无线胶囊内窥镜形成的胶囊坐标系{c}分别绕预设的世界坐标系{w}的x轴和y轴旋转的角度(α，β)，这可以由惯性传感器直接测量得到。因此，需要优化求解的未知参数包括无线胶囊内窥镜的三维位置和胶囊坐标系{c}绕世界坐标系{w}的z轴旋转的角度γ，上述未知参数和二轴旋转数据(α，β)共同构成了无线胶囊内窥镜的实时六维位姿。
[0098]
在具体实现中，上述未知参数pc，γ可以通过非线性最小二乘优化求解，即最小化多个磁传感器测量得到的分离磁场值与理论磁场值之间的误差的平方和：
[0099]
[0100]
为了求解上述非线性最小二乘估计问题，可以采用l-m(levenberg-marquardt)算法对上式进行求解，得到pc，γ的最优估计，从而获得无限胶囊内窥镜在人体内的精确六维位姿。
[0101]
在本技术实施例中，由于方向角γ是以2π为周期的函数，在求解出方向角γ后，还可以将其变换至(-π，π)的主周期内。
[0102]
本技术实施例提供的定位系统使用基于永磁铁-电磁线圈的复合式体外驱动器，并结合胶囊内置的多个磁传感器，用于对磁驱动下的无线胶囊内窥镜的实时六自由度定位。本技术实施例提供的定位系统仅在胶囊内部采用一个微型低功耗的惯性传感器和三个以特殊排布方式安装的磁传感器，不需要在体外设置传感器即可进行胶囊的实时六维定位，且用电量低于一般的内部定位机构，减小了胶囊的体积和重量，提高了电池利用率。工作空间可扩大到机械臂的整个工作空间，不受体外传感器安装位置的限制。
[0103]
其次，本技术实施例提供的定位系统中，体外驱动器采用两个正交的电磁线圈套着一个永磁铁的安装方式，结构简单且易于安装，相较于其他使用外部磁传感器的方案更加灵活，有助于提供更大的工作空间，而不要求被检查者置身于一个固定的传感器阵列结构中，提高了定位的准确性和稳定性。
[0104]
第三，本技术实施例提供的定位系统中，由机械臂某端持一个球形的体外驱动器，通过改变体外驱动器内永磁铁磁场来控制胶囊在体内的运动，装置简单且易于安装，相较于仅依赖电磁线圈的驱动装置更加小巧轻便，可提供更大的工作空间，体外驱动器的球形表面更适合与病人体表接触、降低安全风险。
[0105]
第四，本技术实施例通过对体外驱动器内永磁铁和电磁线圈的磁场及胶囊内被动永磁铁的磁场建立数学模型，并基于体外磁源产生磁场控制胶囊，使用胶囊内置的磁传感器感知磁场，使用胶囊内置的惯性传感器感知胶囊旋转，并使用信号处理算法和非线性优化算法对胶囊进行实时的六维位姿定位。该定位算法不受驱动磁场影响，定位结果为胶囊相对于固定世界坐标系的六维位姿，更加直观和便于使用，并可以实现更高的定位频率。
[0106]
参照图8，示出了本技术实施例提供的一种无线胶囊内窥镜的定位装置的示意图，具体可以包括获取模块801、计算模块802、分离模块803和定位模块804，其中：
[0107]
获取模块801，用于在控制体外驱动器驱动所述无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
[0108]
计算模块802，用于确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
[0109]
分离模块803，用于对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
[0110]
定位模块804，用于基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
[0111]
在本技术实施例中，多个所述磁传感器以特定排布方式安装于所述无线胶囊内窥镜中，所述胶囊永磁铁产生的磁场在以特定排布方式安装的多个所述磁传感器上的分量为
零。
[0112]
在本技术实施例中，多个所述磁传感器包括第一磁传感器、第二磁传感器和第三磁传感器，所述胶囊永磁铁可以为圆柱形永磁铁，所述胶囊永磁铁沿胶囊自身轴线方向充磁，基于所述无线胶囊内窥镜形成一胶囊坐标系，所述第一磁传感器安装于所述胶囊坐标系的x轴上一点且垂直于所述胶囊坐标系的x轴方向，所述第二磁传感器安装于所述胶囊坐标系的y轴上一点且垂直于所述胶囊坐标系的y轴方向，所述第三磁传感器安装于所述胶囊永磁铁产生的磁感线切线斜率为0的一点且垂直于所述胶囊坐标系的z轴方向。
[0113]
在本技术实施例中，所述第一电磁线圈和所述第二电磁线圈的轴线正交，所述体外永磁铁可以为圆柱形永磁铁，沿自身轴向方向充磁，所述体外永磁铁嵌套于所述第一电磁线圈和所述第二电磁线圈内，基于所述体外驱动器形成一驱动器坐标系，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩分别与所述驱动器坐标系的各轴方向相平行。
[0114]
在本技术实施例中，所述体外驱动器的外壳形状为球形，所述体外驱动器的外壳材质可以为塑料材质，所述体外驱动器安装于机械臂的末端执行器上，所述体外永磁铁的充磁方向与所述机械臂末端关节旋转轴方向正交。
[0115]
在本技术实施例中，所述体外驱动器的位置数据为所述体外驱动器在预设的世界坐标系下的位置数据，所述计算模块802具体可以用于：分别确定每个所述磁传感器在基于所述无线胶囊内窥镜形成的胶囊坐标系下的位置和法向量方向；根据所述二轴旋转数据和每个所述磁传感器在所述胶囊坐标系下的位置和法向量方向，确定每个所述磁传感器在所述世界坐标系下的位置和法向量方向；基于每个所述磁传感器在所述世界坐标系下的位置和法向量方向，计算每个所述体外磁源在每个所述磁传感器处的理论磁场值。
[0116]
在本技术实施例中，所述计算模块802还可以用于：基于磁偶极子模型和每个所述磁传感器在所述世界坐标系下的位置和法向量方向，计算每个所述体外磁源产生的磁场在每个所述磁传感器在所述世界坐标系下的法向量方向上的投影，得到每个所述体外磁源在每个所述磁传感器处的理论磁场值。
[0117]
在本技术实施例中，所述分离模块803具体可以用于：控制向所述第一电磁线圈通以第一频率的周期方波电流，以使所述第一电磁线圈产生所述第一频率的周期时变磁场；以及，控制向所述第二电磁线圈通以第二频率的周期方波电流，以使所述第二电磁线圈产生所述第二频率的周期时变磁场；按照预设采样频率对预设时间段内每个所述磁传感器测量到的所述测量磁场值进行采样，分别得到与每个所述磁传感器对应的测量磁场值采样序列；分别计算每个所述测量磁场值采样序列的时间平均值，将所述时间平均值作为所述体外永磁铁在对应的所述磁传感器处的分离磁场值；分别采用每个所述测量磁场值采样序列减去对应的所述磁传感器的时间平均值，得到所述第一电磁线圈和所述第二电磁线圈产生磁场在每个所述磁传感器处的测量磁场值之和的时间序列；基于所述测量磁场值之和的时间序列对所述第一电磁线圈和所述第二电磁线圈产生的测量磁场值进行分离，得到所述第一电磁线圈和所述第二电磁线圈在每个所述磁传感器处的分离磁场值。
[0118]
在本技术实施例中，所述分离模块803还可以用于：分别对所述第一电磁线圈和所述第二电磁线圈上的电流进行采样，得到所述第一电磁线圈和所述第二电磁线圈的电流采样结果；分别对所述电流采样结果进行快速傅里叶变换，得到电流相位估计，所述电流相位
估计包括所述第一电磁线圈的第一电流相位估计和所述第二电磁线圈的第二电流相位估计；对所述测量磁场值之和的时间序列进行快速傅里叶变换，得到相位估计和幅值估计，所述相位估计包括所述第一电磁线圈的第一相位估计和所述第二电磁线圈的第二相位估计，所述幅值估计包括所述第一电磁线圈的第一幅值估计和所述第二电磁线圈的第二幅值估计；根据所述电流相位估计、所述相位估计和所述幅值估计，计算所述第一电磁线圈和所述第二电磁线圈在每个所述磁传感器处的分离磁场值。
[0119]
在本技术实施例中，所述分离模块803还可以用于：根据所述电流相位估计和所述相位估计，分别计算所述第一电磁线圈和所述第二电磁线圈的相位差的绝对值；根据所述相位差的绝对值，确定所述第一电磁线圈和所述第二电磁线圈的测量磁场值符号；将所述测量磁场值符号分别与所述幅值估计相乘，得到所述第一电磁线圈和所述第二电磁线圈在每个所述磁传感器处的分离磁场值。
[0120]
在本技术实施例中，所述分离模块803还可以用于：若所述第一电磁线圈的相位差的绝对值介于0-0.5π或1.5π-2π之间，则确定所述第一电磁线圈的测量磁场值符号为正；若所述第一电磁线圈的相位差的绝对值介于0.5π-1.5π之间，则确定所述第一电磁线圈的测量磁场值符号为负；若所述第二电磁线圈的相位差的绝对值介于0-0.5π或1.5π-2π之间，则确定所述第二电磁线圈的测量磁场值符号为正；若所述第二电磁线圈的相位差的绝对值介于0.5π-1.5π之间，则确定所述第二电磁线圈的测量磁场值符号为负。
[0121]
在本技术实施例中，所述二轴旋转数据为基于所述无线胶囊内窥镜形成的胶囊坐标系分别绕预设的世界坐标系的x轴和y轴旋转的角度，所述定位模块804具体可以用于：基于所述理论磁场值和所述分离磁场值，采用最小二乘法求解所述无线胶囊内窥镜的六维位姿中的未知参数，所述未知参数包括所述无线胶囊内窥镜的三维位置和所述胶囊坐标系绕所述世界坐标系的z轴旋转的角度，所述未知参数和所述二轴旋转数据共同构成所述无线胶囊内窥镜的实时六维位姿。
[0122]
对于装置实施例而言，由于其与系统实施例中对于计算机设备的介绍的内容基本相似，所以描述得比较简单，相关之处参见系统实施例部分的说明即可。
[0123]
参照图9，示出了本技术实施例提供的一种计算机设备的示意图。如图9所示，本技术实施例中的计算机设备900包括：处理器910、存储器920以及存储在所述存储器920中并可在所述处理器910上运行的计算机程序921。所述处理器910执行所述计算机程序921时实现上述系统实施例中计算机设备所实现的步骤。或者，所述处理器910执行所述计算机程序921时实现上述各装置实施例中各模块/单元的功能，例如图8所示模块801至804的功能。
[0124]
示例性的，所述计算机程序921可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器920中，并由所述处理器910执行，以完成本技术。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段可以用于描述所述计算机程序921在所述计算机设备900中的执行过程。例如，所述计算机程序921可以被分割成获取模块、计算模块、分离模块和定位模块，各模块具体功能如下：
[0125]
获取模块，用于在控制体外驱动器驱动所述无线胶囊内窥镜运动的过程中，获取所述无线胶囊内窥镜中内置的惯性传感器测量到的所述无线胶囊内窥镜的二轴旋转数据以及所述无线胶囊内窥镜中内置的每个磁传感器测量到的测量磁场值；
[0126]
计算模块，用于确定所述体外驱动器的位置数据，并根据所述二轴旋转数据和所
述位置数据计算所述体外驱动器中内置的多个体外磁源在每个所述磁传感器处的理论磁场值，多个所述体外磁源包括体外永磁铁、第一电磁线圈和第二电磁线圈，所述体外永磁铁、所述第一电磁线圈和所述第二电磁线圈的单位磁矩两两正交；
[0127]
分离模块，用于对每个所述磁传感器测量到的所述测量磁场值进行分离，得到每个所述体外磁源在每个所述磁传感器处的分离磁场值；
[0128]
定位模块，用于基于所述理论磁场值和所述分离磁场值对所述无线胶囊内窥镜进行定位，输出所述无线胶囊内窥镜的实时六维位姿。
[0129]
在本技术实施例中，多个所述磁传感器以特定排布方式安装于所述无线胶囊内窥镜中，所述胶囊永磁铁产生的磁场在以特定排布方式安装的多个所述磁传感器上的分量为零。
[0130]
所述计算机设备900可以是前述各个实施例中的计算机设备，该计算机设备900可以是桌上型计算机、云端服务器等计算设备。所述计算机设备900可包括，但不仅限于，处理器910、存储器920。本领域技术人员可以理解，图9仅仅是计算机设备900的一种示例，并不构成对计算机设备900的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述计算机设备900还可以包括输入输出设备、网络接入设备、总线等。
[0131]
所述处理器910可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0132]
所述存储器920可以是所述计算机设备900的内部存储单元，例如计算机设备900的硬盘或内存。所述存储器920也可以是所述计算机设备900的外部存储设备，例如所述计算机设备900上配备的插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)等等。进一步地，所述存储器920还可以既包括所述计算机设备900的内部存储单元也包括外部存储设备。所述存储器920用于存储所述计算机程序921以及所述计算机设备900所需的其他程序和数据。所述存储器920还可以用于暂时地存储已经输出或者将要输出的数据。
[0133]
以上所述实施例仅用以说明本技术的技术方案，而非对其限制。尽管参照前述实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的精神和范围，均应包含在本技术的保护范围之内。