技术领域
[0001] 本
发明属于三维定位技术领域，尤其涉及一种胶囊内窥镜及其定位方法及系统。
背景技术
[0002] 胶囊内窥镜，又称智能胶囊式内窥镜系统，为胶囊形状的内窥镜。胶囊内窥镜内置有摄像与
信号传输装置，经受检者口服，在消化道内运动并拍摄图像，以获得受检者消化道内部的图像。相较于传统的内窥镜，胶囊内窥镜不会给受检者带来创伤，不影响受检者的活动，十分便利。为了更精确得控制胶囊内窥镜对人体消化道系统进行检测，需要实时定位胶囊内窥镜在人体内的
位置。
发明内容
[0003] 本发明
实施例提供一种胶囊内窥镜及其定位方法及系统，旨在提高胶囊内窥镜的定位准确度。
[0004] 本发明实施例提供了一种胶囊内窥镜的定位方法，所述胶囊内窥镜内部设有磁
传感器和所述胶囊内窥镜中的磁体，所述方法包括：
[0005] 通过所述
磁传感器对驱动所述胶囊内窥镜移动的驱动磁体进行
磁场测量，获得所述驱动磁体的磁场测量值；
[0006] 测量所述胶囊内窥镜的自旋
角度，获得所述胶囊内窥镜的自旋角度的测量值；
[0007] 依据所述驱动磁体的磁场测量值、所述自旋角度的测量值、预设驱动磁场模型和预设优化
算法，计算所述胶囊内窥镜相对所述驱动磁体的
位姿信息；
[0008] 根据所述驱动磁体在世界
坐标系的位姿信息和所述胶囊内窥镜相对所述驱动磁体的位姿信息，确定所述胶囊内窥镜在世界坐标系的位姿信息。
[0009] 更进一步地，通过磁传感器对驱动所述胶囊内窥镜移动的驱动磁体进行磁场测量的步骤，包括：
[0010] 获取预先测量的第一磁场值，所述第一磁场值为在不受所述驱动磁体的影响下所述胶囊内窥镜内部磁体、
地磁场及周围环境磁场的磁场总值；
[0011] 通过所述磁传感器测量得到第二磁场值，所述第二磁场值为所述驱动磁体、所述胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值；
[0012] 依据所述第一磁场值和所述第二磁场值，计算所述驱动磁体的磁场测量值。
[0013] 更进一步地，计算所述胶囊内窥镜相对所述驱动磁体的位姿信息的步骤，包括：
[0014] 依据所述驱动磁场模型、所述驱动磁体的磁场测量值和所述自旋角度的测量值，计算所述驱动磁体的磁场测量误差；
[0015] 通过所述
优化算法对所述驱动磁体的磁场测量误差进行最小化，获得所述胶囊内窥镜相对所述驱动磁体的位姿信息。
[0016] 更进一步地，通过磁传感器对驱动所述胶囊内窥镜移动的驱动磁体进行磁场测量的步骤之前，所述方法还包括：
[0017] 获取所述磁传感器与所述胶囊内窥镜中心的距离，并获取所述驱动磁体的相对磁导率；
[0018] 根据磁偶极子模型、所述磁传感器与所述胶囊内窥镜中心的距离和所述驱动磁体的相对磁导率，构建所述驱动磁场模型。
[0019] 更进一步地，所述胶囊内窥镜内部设有至少两个所述磁传感器，所述磁传感器为三轴磁传感器。
[0020] 更进一步地，所述胶囊内窥镜内部设有惯性传感器，所述惯性传感器用来测量所述胶囊内窥镜的自旋角度。
[0021] 本发明实施例还提供了一种胶囊内窥镜，包括：设置在所述胶囊内窥镜中的磁传感器、胶囊内窥镜中的磁体和自旋角度
测量传感器；
[0022] 所述磁传感器用于对驱动所述胶囊内窥镜移动的驱动磁体进行磁场测量，获得所述驱动磁体的磁场测量值；
[0023] 所述磁传感器包括第一磁传感器和第二磁传感器，所述第一磁传感器和所述第二磁传感器分散
焊接设置在所述胶囊内窥镜中的
电路板上；
[0024] 所述自旋角度测量传感器用于测量所述胶囊内窥镜的自旋角度，从而定位系统根据从所述磁传感器处获得的所述自旋角度的测量值，结合预设驱动磁场模型和预设优化算法，计算所述胶囊内窥镜相对于所述驱动磁体的位姿信息，根据所述驱动磁体在世界坐标系的位姿信息和所述胶囊内窥镜相对于所述驱动磁体的位置信息，确定所述胶囊内窥镜在世界坐标系的位姿信息。
[0025] 更进一步地，所述第一次传感器或所述第二磁传感为三轴磁传感器。
[0026] 更进一步地，所述自旋角度测量传感器包括惯性传感器。
[0027] 更进一步地，所述磁传感器包括两个或两个以上分散的所述第一传感器或所述第二传感器。
[0028] 本发明实施例提供了一种定位系统，所述定位系统用于定位胶囊内窥镜在世界坐标系的位姿信息，所述胶囊内窥镜包括：设置在所述胶囊内窥镜中的磁传感器、胶囊内窥镜中的磁体和自旋角度测量传感器；所述磁传感器用于对驱动所述胶囊内窥镜移动的驱动磁体进行磁场测量，获得所述驱动磁体的磁场测量值；所述磁传感器包括第一磁传感器和第二磁传感器，所述第一磁传感器和所述第二磁传感器分散焊接设置在所述胶囊内窥镜中的
电路板上；所述自旋角度测量传感器用于测量所述胶囊内窥镜的自旋角度；
[0029] 所述定位系统包括：接收器和处理装置；
[0030] 所述接收器，用于从所述磁传感器处获取所述驱动磁体的磁场测量值，以及从所述自旋角度测量传感器处获得的所述自旋角度的测量值；
[0031] 所述处理装置，用于根据所述磁传感器处获得的所述驱动磁体的磁场测量值，以及根据从所述自旋角度测量传感器处获得的所述自旋角度的测量值，结合预设驱动磁场模型和预设优化算法，计算所述胶囊内窥镜相对所述驱动磁体的位姿信息，根据所述驱动磁体在世界坐标系的位姿信息。
[0032] 更进一步地，所述胶囊内窥镜包括射频模
块，所述第一磁传感器、所述第二磁传感器和所述自旋角度测量传感器与所述射频模块连接，所述第一磁传感器和所述第二磁传感器分别通过所述射频模块将所述驱动磁体的磁场测量值发送至所述接收器，所述自旋角度测量传感器通过所述射频模块发送所述自旋角度的测量值至所述接收器。
[0033] 更进一步地，所述处理装置还用于依据所述驱动磁场模型、所述驱动磁体的磁场测量值和所述自旋角度的测量值，计算所述驱动磁体的磁场测量误差；通过所述优化算法对所述驱动磁体的磁场测量误差进行最小化，获得所述胶囊内窥镜相对所述驱动磁体的位姿信息。
[0034] 更进一步地，所述处理装置还用于获取所述磁传感器与所述胶囊内窥镜中心的距离，并获取所述驱动磁体的相对磁导率；根据磁偶极子模型、所述磁传感器与所述胶囊内窥镜中心的距离和所述驱动磁体的相对磁导率，构建所述驱动磁场模型。
[0035] 本发明实施例还提供一种对胶囊内窥镜进行定位的定位系统，所述胶囊内窥镜内部设有磁传感器和所述胶囊内窥镜中的磁体，所述定位系统包括：
[0036] 磁场测量单元，用于通过磁传感器对驱动所述胶囊内窥镜移动的驱动磁体进行磁场测量，获得所述驱动磁体的磁场测量值；
[0037] 自旋测量单元，用于测量所述胶囊内窥镜的自旋角度，获得所述胶囊内窥镜的自旋角度的测量值；
[0038] 相对位姿计算单元，用于依据所述驱动磁体的磁场测量值、所述自旋角度的测量值、预设驱动磁场模型和预设优化算法，计算所述胶囊内窥镜相对所述驱动磁体的位姿信息；以及
[0039] 胶囊位姿确定单元，用于根据所述驱动磁体在世界坐标系的位姿信息和所述胶囊内窥镜相对所述驱动磁体的位姿信息，确定所述胶囊内窥镜在世界坐标系的位姿信息。
[0040] 更进一步地，所述磁场测量单元包括：
[0041] 第一磁场值获取单元，用于获取预先测量的第一磁场值，所述第一磁场值为在不受所述驱动磁体的影响下所述胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值；
[0042] 第二磁场值测量单元，用于通过所述磁传感器测量得到第二磁场值，所述第二磁场值为所述驱动磁体、所述胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值；以及[0043] 磁场值计算单元，用于依据所述第一磁场值和所述第二磁场值，计算所述驱动磁体的磁场测量值。
[0044] 更进一步地，所述相对位姿计算单元包括：
[0045] 误差计算单元，用于依据所述驱动磁
体模型、所述驱动磁体的磁场测量值和所述自旋角度的测量值，计算所述驱动磁体的磁场测量误差；以及
[0046] 误差最小化单元，用于通过所述优化算法对所述驱动磁体的磁场测量误差进行最小化，获得所述胶囊内窥镜相对所述驱动磁体的位姿信息。
[0047] 更进一步地，所述定位系统还包括：
[0048] 参数获取单元，用于获取所述磁传感器与所述胶囊内窥镜中心的距离，并获取所述驱动磁体的相对磁导率；以及
[0049] 磁场模型构建单元，用于根据磁偶极子模型、所述磁传感器与所述胶囊内窥镜中心的距离和所述驱动磁体的相对磁导率，构建所述驱动磁场模型。
[0050] 本发明通过磁传感器对驱动磁体进行磁场测量，获得驱动磁体的磁场测量值，同时测量胶囊内窥镜的自旋角度，获得胶囊内窥镜的自旋角度的测量值，依据磁场测量值、自旋角度的测量值、预设驱动磁场模型和预设优化算法，计算胶囊内窥镜相对驱动磁体的位姿信息，再依据驱动磁体在世界坐标系的位姿信息，获得胶囊内窥镜在世界坐标系的位姿信息，从而依据驱动磁体的磁场测量值、驱动磁场模型、自旋角度的测量值和优化算法，对胶囊内窥镜进行定位，避免了胶囊内窥镜的定位过程受驱动磁体的磁场影响，并提高胶囊内窥镜的定位准确度。
附图说明
[0051] 图1是本发明实施例一提供的一种胶囊内窥镜的定位方法的实现
流程图；
[0052] 图2是本发明实施例一提供的一种胶囊内窥镜的定位方法中惯性传感器三轴方向的示例图；
[0053] 图3是本发明实施例二提供的一种胶囊内窥镜的定位方法的实现流程图；
[0054] 图4是本发明实施例三提供的一种胶囊内窥镜的结构示意图；
[0055] 图5是本发明实施例四提供的一种定位系统的结构示意图；
[0056] 图6是本发明实施例四提供的一种定位系统的优选结构示意图；
[0057] 图7是本发明实施例五提供的一种对胶囊内窥镜进行定位的定位系统的结构示意图；以及
[0058] 图8是本发明实施例五提供的一种对胶囊内窥镜进行定位的定位系统的优选结构示意图。
具体实施方式
[0059] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0060] 目前，定位方式主要有光学定位、
超声波定位、射线定位和磁定位，光学定位不适用于体内定位，
超声波定位
精度不高，射线定位具有
辐射性。胶囊内窥镜内设有小磁体，在人体外部的磁控系统中设有大磁体，大磁体和小磁体都是
永磁体，大磁体作为磁控系统中的驱动磁体，用于提供驱动磁场，在驱动磁场的驱动下胶囊内窥镜在人体消化系统内运动。目前的磁定位技术中，通过测量小磁体的磁场变化，来得到胶囊内窥镜的位置，然而，大磁体磁场强度大，会对小磁体的磁场强度测量造成影响，导致胶囊内窥镜定位准确度不高。在本发明中，通过测量驱动磁体的磁场，并依据驱动磁场模型、胶囊内窥镜的自旋角度和优化算法，确定胶囊内窥镜的位姿信息，有效地避免驱动磁场对定位准确度的影响，提高胶囊内窥镜的定位准确度。
[0061] 实施例一
[0062] 图1示出了本发明实施例一提供的一种胶囊内窥镜的定位方法的实现流程，为了便于说明，仅示出了与本发明实施例相关的部分，详述如下：
[0063] 在步骤S101中，通过磁传感器对驱动胶囊内窥镜移动的驱动磁体进行磁场测量，获得驱动磁体的磁场测量值。
[0064] 在本发明实施例中，胶囊内窥镜内部设有磁传感器和胶囊内窥镜中的磁体，胶囊内窥镜外部的驱动磁体通过与胶囊内窥镜中的磁体之间相互作用，驱动胶囊内窥镜移动。在驱动胶囊内窥镜移动的过程中，需要实时对胶囊内窥镜进行定位，由于胶囊内窥镜位姿发生变化时胶囊内窥镜中的磁体与驱动磁体之间的相对位姿不同，胶囊内窥镜所处位置的磁场也会发生变化。所以，在对胶囊内窥镜进行定位时，通过胶囊内窥镜内部的磁传感器测量驱动磁体的磁场强度，测量得到的值即为驱动磁体的磁场测量值。
[0065] 优选地，在胶囊内窥镜内部设有至少两个磁传感器，以通过多个磁传感器同时进行测量，来提高驱动磁体磁场测量的准确度。作为示例地，驱动磁体的磁场测量值可表示为Vl，其中，l表示第l个磁传感器。
[0066] 当胶囊内窥镜内部设有至少两个磁传感器时，优选地，磁传感器在胶囊内窥镜内部位置分散，磁传感器与胶囊内窥镜内部的磁体分离布置、且距离固定，单个磁传感器的磁场测量范围大于或等于胶囊内窥镜内部两个磁体的磁场强度
叠加值，以提高磁传感器的利用率和测量效果。
[0067] 进一步优选地，磁传感器为三轴磁传感器，可以测量到三个测量轴方向上的磁场强度，以提高磁传感器的测量效果。
[0068] 在步骤S102中，测量胶囊内窥镜的自旋角度，获得胶囊内窥镜的自旋角度的测量值。
[0069] 在本发明实施例中，在获得驱动磁体的磁场测量值后，由于胶囊内窥镜的位置为三维位置，即包括三个
自由度，胶囊内窥镜的
姿态为三维空间的姿态，也包括三个自由度，为了后续能够更为准确地确定胶囊内窥镜的位姿信息(位置和姿态)，即确定6个自由度的值，需要测量胶囊内窥镜的自旋角度。可通过胶囊内窥镜中的自旋角度测量传感器测量胶囊内窥镜的自旋角度。
[0070] 优选地，在胶囊内窥镜内部设有惯性传感器，通过惯性传感器来测量胶囊内窥镜的自旋角度，以提高胶囊内窥镜自旋角度的测量准确度。
[0071] 具体地，惯性传感器包括线
加速度传感器和
陀螺仪。
[0072] 当惯性传感器中的线加速度传感器为三轴线加速度传感器和三轴陀螺仪时，如图2所示，ax、ay、az分别表示三轴线加速度传感器的三个测量轴，ωx、ωy、ωz分别表示三轴陀螺仪的三个测量轴，az、ωz的方向为H0方向，acc为惯性传感器受到的线加速度，θ为胶囊内窥镜的自旋角度， 为az与acc之间的夹角。三轴线加速度传感器在三个测量轴上的分量分别为：
进而求得胶囊内窥镜的自旋角度的测量值为θ＝arctan(-accy/accx)。通过惯性传感器中的三轴陀螺仪来计算胶囊内窥镜的自旋角度的测量值的公式为 其中，N为
三轴陀螺仪测量到数据的个数，dt为三轴陀螺仪的
数据采集间隔，ωzi为ωz测量轴上测量胶囊内窥镜自旋的角速率数据点。在采用惯性传感器测量胶囊内窥镜的自旋角度时，可单独采用线加速度传感器或陀螺仪，也可以结合两者一起测量，其中，线加速度传感器为三轴线加速度传感器，陀螺仪测量轴的数量不作限制。在计算得到自旋角度的测量值θ后，需计算关于自旋角度的测量值θ的坐标变换关系R(θ)，依据磁传感器的轴向与惯性传感器的轴向，R(θ)的表示形会有所变化，当磁传感器的轴向与惯性传感器的轴向相同时，即磁传感器的轴向Bx、By、Bz分别对应惯性传感器的轴向ax(或ωx)、ay(或ωy)、az(或wz)，可得到关于自旋角度的测量值θ的坐标变换关系R(θ)为：
[0073] 其中，R(θ)用于后续求解胶囊内窥镜六个自由度的位姿信息。
[0074] 在步骤S103中，依据驱动磁体的磁场测量值、自旋角度的测量值、预设驱动磁场模型和预设优化算法，计算胶囊内窥镜相对驱动磁体的位姿信息。
[0075] 在本发明实施例中，预先构建驱动磁场模型，依据驱动磁场模型、驱动磁体的磁场测量值和自旋角度的测量值，来计算驱动磁体的磁场测量误差，通过优化算法对磁场测量误差进行最小化，可计算得到胶囊内窥镜相对于驱动磁体的位姿信息。
[0076] 优选地，在构建驱动磁场模型时，获取各磁传感器与胶囊内窥镜中心的距离(xl,yl,zl)T，获取驱动磁体的相对磁导率μr，磁偶极子模型在磁定位运算中具有较高的运算效率，很适合实时定位，依据获得的这些参数和磁偶极子模型，构建驱动磁场模型，驱动磁场模型表示为：
[0077] 其中，胶囊内窥镜中心与驱动磁体中心的相对T T
位置为(a,b,c) ，胶囊内窥镜中心与驱动磁体中心的相对姿态为H0＝(m,n,p) ，Pl表示第l个磁传感器与驱动磁体中心的距离且Pl＝(xl-a,yl-b,zl-c)T，Rl为Pl的模，存在约束条件m2+n2+p2＝1，μ0为
真空磁导率，MT为表征磁体磁场强度的常数，Bl为驱动磁场的磁场模型值。求解胶囊内窥镜相对驱动磁体的位姿信息，即求解(a,b,c)T和H0＝(m,n,p)T。
[0078] 在依据驱动磁场模型、驱动磁体的磁场测量值和自旋角度的测量值，计算驱动磁体的磁场测量误差Err时，进一步优选地，磁场测量误差表示为：
[0079] 以提高磁场测量误差的计算准确度，并能够通过误差最小化过程计算得到胶囊内窥镜相对驱动磁体的位姿信息。在通过优化算法对磁场测量误差进行最小化时，进一步优选地，优选算法采用非线性最小二乘法-LM算法或智能算法-PSO算法，以提高磁场测量误差的最小化效果。
[0080] 在步骤S104中，根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，确定胶囊内窥镜在世界坐标系的位姿信息。
[0081] 在本发明实施例中，驱动磁体在世界坐标系的位姿信息(位置和姿态)是已知的，在获得胶囊内窥镜相对驱动磁体的位姿信息，即可根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，转换得到胶囊内窥镜在世界坐标系的位姿信息，完成对胶囊内窥镜的定位。
[0082] 优选地，在主机
用户界面建立胶囊内窥镜模型，根据定位得到的胶囊内窥镜的位姿信息，刷新胶囊内窥镜模型的位姿信息，在接收到用户输入的下一时刻位姿信息时，可通过调整驱动磁体的位置姿态产生磁
力，使得胶囊内窥镜运动到下一时刻位姿信息中的位置和姿态，从而实现胶囊内窥镜定位和运动的
人机交互过程，提高用户体验。此外，当胶囊内窥镜运动到下一时刻位姿信息中的位置和姿态时，根据胶囊内窥镜实时反馈的位姿信息，对胶囊内窥镜的位姿进行不断调整，以平衡胶囊内窥镜的受力，使得胶囊内窥镜实现悬浮。
[0083] 在本发明实施例中，通过测量驱动磁体的磁场，测量胶囊内窥镜的自旋角度，依据驱动磁场模型、驱动磁体的磁场测量值、胶囊内窥镜的自旋角度的测量值和优化算法，对驱动磁体的磁场测量误差进行最小化，以确定胶囊内窥镜的位姿信息，从而有效地避免驱动磁场对定位准确度的影响，提高胶囊内窥镜的定位准确度。
[0084] 实施例二
[0085] 图3示出了本发明实施例二提供的一种胶囊内窥镜的定位方法的实现流程，为了便于说明，仅示出了与本发明实施例相关的部分，详述如下：
[0086] 在步骤S301中，获取预先测量的第一磁场值。
[0087] 在本发明实施例中，在没有驱动磁体的磁场时(即胶囊内窥镜位于驱动磁体的磁场范围之外时)，通过胶囊内窥镜内部的磁传感器测量第一磁场值，第一磁场值为胶囊内窥镜中的磁体、地磁场及周围环境磁场的磁场总值，周围环境磁场为胶囊内窥镜周围设备(例如一些医疗设备，或者用户的手机、电脑等设备)的磁场，不包括驱动磁体的磁场。其中，在不考虑周围环境磁场变化时，第一磁场值为固定值。
[0088] 在步骤S302中，通过磁传感器测量得到第二磁场值。
[0089] 在本发明实施例中，在驱动磁体驱动胶囊内窥镜移动时，胶囊内窥镜位于驱动磁体的磁场范围内，通过胶囊内窥镜内部的磁传感器测量第二磁场值，第二磁场值为驱动磁体、胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值。
[0090] 在步骤S303中，依据第一磁场值和第二磁场值，计算驱动磁体的磁场测量值。
[0091] 在本发明实施例中，第一磁场值为胶囊内窥镜中的磁体、地磁场及周围环境磁场的磁场总值，第二磁场值为驱动磁体、胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值，因此可通过将第二磁场值减去第一磁场值，计算得到驱动磁体的磁场测量值Vl。
[0092] 优选地，驱动磁体的磁场测量值Vl的计算公式为：
[0093] Vl＝Vl1-Vl0，其中，Vl0为第l个磁传感器测量到的第一磁场值，Vl1为第l个磁传感器测量到的第二磁场值。
[0094] 在步骤S304中，测量胶囊内窥镜的自旋角度，获得胶囊内窥镜的自旋角度的测量值。
[0095] 在步骤S305中，依据驱动磁体的磁场测量值、自旋角度的测量值、预设驱动磁场模型和预设优化算法，计算胶囊内窥镜相对驱动磁体的位姿信息。
[0096] 在步骤S306中，根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，确定胶囊内窥镜在世界坐标系的位姿信息。
[0097] 在本发明实施例中，步骤S304至步骤S306的详细内容可参照实施例一种步骤S102至步骤S104的描述，在此不再赘述。
[0098] 在本发明实施例种，通过获取驱动磁体磁场存在时和驱动磁体磁场不存在时，胶囊内窥镜内部磁传感器分别测量到的测量值，计算得到驱动磁体的磁场测量值，依据驱动磁场模型、驱动磁体的磁场测量值、胶囊内窥镜的自旋角度的测量值和优化算法，对驱动磁体的磁场测量误差进行最小化，获得胶囊内窥镜的位姿信息，从而无需依赖胶囊内窥镜内部磁体的磁场测量值来实现胶囊内窥镜的定位，有效地避免驱动磁场对定位准确度的影响，提高胶囊内窥镜的定位准确度。
[0099] 实施例三
[0100] 图4示出了本发明实施例三提供的一种胶囊内窥镜40，该胶囊内窥镜40适用于上述实施例一和实施例二示出的胶囊内窥镜的定位方法。为了便于说明，仅示出与本发明实施例相关的部分。
[0101] 在本发明实施例中，胶囊内窥镜40包括设置于胶囊内窥镜中的磁传感器41、胶囊内窥镜中的磁体42和自旋角度测量传感器43。其中：
[0102] 磁传感器41用于对驱动胶囊内窥镜40移动的驱动磁体进行测量，获得驱动磁体的磁场测量值，磁传感器41包括第一磁传感器411和第二磁传感器412，第一磁传感器411和第二磁传感器412分散焊接设置在胶囊内窥镜40的电路板上；
[0103] 自旋角度测量传感器43用于测量胶囊内窥镜40的自旋角度。
[0104] 在本发明实施例中，用于对胶囊内窥镜40进行定位的定位系统根据从磁传感器41处获取到的驱动磁体的磁场测量值，以及根据从自旋角度测量传感器43处获得的自旋角度的测量值，结合预设驱动磁场模型和预设优化算法，计算胶囊内窥镜40相对驱动磁体的位姿信息，根据驱动磁体在世界坐标系的位置信息和胶囊内窥镜40相对驱动磁体的位姿信息，确定胶囊内窥镜40在世界坐标系的位姿信息。其中，驱动磁场模型、优化算法和胶囊内窥镜40相对驱动磁体的位姿信息的计算过程可参照实施例一相应步骤的描述，在此不再赘述。
[0105] 在本发明实施例中，第一磁传感器411和第二磁传感器412分散焊接设置在胶囊内窥镜40的电路板上，以同时测量到胶囊内窥镜40内部不同位置处的磁场值，进而在对胶囊内窥镜40进行定位时依据第一传感器411和第二传感器412分别获取的驱动磁体的磁场测量值进行定位，提高胶囊内窥镜40的定位准确度。优选地，磁传感器41包括两个或两个以上分散的第一传感器411和第二传感器412，从而通过多个分散的磁传感器41来测量到胶囊内窥镜40内部不同位置处的磁场值，提高胶囊内窥镜40的定位准确度。其中，驱动磁体的磁场测量值的获取过程可参照实施例一和实施例二相应步骤的详细描述，在此不再赘述。
[0106] 优选地，第一磁传感器411或第二磁传感器412为三轴磁传感器，可以测量到三个测量轴方向上的磁场值，结合三个测量轴方向的磁场值和自旋角度测量传感器43测量到的胶囊内窥镜40的自旋角度，可以实现对胶囊内窥镜40位姿(六个自由度)的定位。
[0107] 优选地，自旋角度测量传感器43包括惯性传感器，通过惯性传感器来测量胶囊内窥镜40的自旋角度，以提高胶囊内窥镜40自旋角度的测量准确度。此外，自旋角度测量传感器43还可为线加速度传感器和/或陀螺仪。进一步地，线加速度传感器为三轴线加速度传感器。
[0108] 优选地，胶囊内窥镜40中还设有射频模块，射频模块与第一磁传感器411、第二磁传感器412和自旋角度测量传感器43连接，用于将第一磁传感器411和第二磁传感器412分别测量到的驱动磁场的磁场测量值发送出去，并将自旋角度测量传感器43测量到的胶囊内窥镜40自旋角度的测量值发送出去，以便外界与胶囊内窥镜40连接的处理器能够及时对这些测量数据进行处理。除射频模块以外，还可以通过能够实现数据通信的其它通信模块(例如蓝牙模块)实现。
[0109] 在本发明实施例中，胶囊内窥镜包括设置于胶囊内窥镜中的磁传感器、胶囊内窥镜中的磁体和自旋角度测量传感器，驱动磁体与胶囊内窥镜中的磁体相互作用，驱动胶囊内窥镜移动，依据胶囊内窥镜中的磁传感器获取的驱动磁场测量值、胶囊内窥镜中的自旋角度测量传感器获取的自旋角度的测量值、结合预设驱动磁场模型和预设优化算法，实现对胶囊内窥镜的定位，并提高了胶囊内窥镜的定位效果。
[0110] 实施例四
[0111] 图5示出了本发明实施例四提供的一种定位系统50的结构，该定位系统50用于定位实施例三中的胶囊内窥镜在世界坐标系的位姿信息，为了便于说明，仅示出了与本发明实施例相关的部分。其中，定位系统50包括：接收器51和处理装置52；
[0112] 接收器51，用于从磁传感器处获取驱动磁体的磁场测量值，以及从自旋角度测量传感器处获得的自旋角度的测量值；
[0113] 处理装置52，用于根据从磁传感器处获取到的驱动磁体的磁场测量值，以及根据从自旋角度测量传感器处获得的自旋角度的测量值，结合预设驱动磁场模型和预设优化算法，计算胶囊内窥镜相对驱动磁体的位姿信息，根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，确定胶囊内窥镜在世界坐标系的位姿信息。
[0114] 在本发明实施例中，处理装置52还用于获取磁传感器与胶囊内窥镜中心的距离，并获取驱动磁体的相对磁导率，根据磁偶极子模型、磁传感器与胶囊内窥镜中心的距离和驱动磁体的相对磁导率，构建驱动磁场模型。具体的构建过程可参照实施例一相应步骤的详细描述，在此不再赘述。
[0115] 在本发明实施例中，处理装置52还用于依据驱动磁场模型、驱动磁体的磁场测量值和自旋角度的测量值，计算驱动磁体的磁场测量误差；通过优化算法对驱动磁体的磁场测量误差进行最小化，获得胶囊内窥镜相对驱动磁体的位姿信息。具体的计算过程可参照实施例一相应步骤的详细描述，在此不再赘述。
[0116] 优选地，如图6所示，定位系统50还包括实施例三中的胶囊内窥镜40，定位系统50用于实时定位胶囊内窥镜40的位姿，其中，胶囊内窥镜40中包括磁传感器41、胶囊内窥镜中的磁体42和自旋角度测量传感器43。胶囊内窥镜40中可设有多个第一磁传感器411和多个第二磁传感器412。
[0117] 优选地，如图6所示，胶囊内窥镜40中还设有射频模块，射频模块与第一磁传感器411、第二磁传感器412和自旋角度测量传感器43连接，用于将第一磁传感器411和第二磁传感器412分别测量到的驱动磁场的磁场测量值发送至定位系统50的接收器51，并将自旋角度测量传感器43测量到的胶囊内窥镜40自旋角度的测量值发送至定位系统50的接收器51，以辅助定位系统50实现对胶囊内窥镜40的定位。除射频模块以外，还可以通过能够实现数据通信的其它通信模块(例如蓝牙模块)实现。
[0118] 在本发明实施例中，定位系统中的接收器接收胶囊内窥镜发送的驱动磁体的磁场测量值和胶囊内窥镜自旋角度的测量值，由处理装置依据驱动磁体的磁场测量值、胶囊内窥镜自旋角度的测量值，结合预设驱动磁场模型、预设优化算法和驱动磁体在世界坐标系中的位姿信息，确定胶囊内窥镜在世界坐标系的位姿信息，从而实现对胶囊内窥镜的定位，并提高了胶囊内窥镜定位的准确度。
[0119] 实施例五
[0120] 图7示出了本发明实施例五提供的一种对胶囊内窥镜进行定位的定位系统的结构，为了便于说明，仅示出了与本发明实施例相关的部分，其中包括：
[0121] 磁场测量单元71，用于通过磁传感器对驱动胶囊内窥镜移动的驱动磁体进行磁场测量，获得驱动磁体的磁场测量值。
[0122] 在本发明实施例中，胶囊内窥镜内部设有磁传感器和胶囊内窥镜中的磁体，胶囊内窥镜外部的驱动磁体通过与胶囊内窥镜中的磁体之间相互作用，驱动胶囊内窥镜移动。在驱动胶囊内窥镜移动的过程中，需要实时对胶囊内窥镜进行定位，由于胶囊内窥镜位姿发生变化时胶囊内窥镜中的磁体与驱动磁体之间的相对位姿不同，胶囊内窥镜所处位置的磁场也会发生变化。所以，在对胶囊内窥镜进行定位时，通过胶囊内窥镜内部的所有磁传感器实现对驱动磁体磁场强度的测量，测量得到的值即为驱动磁体的磁场测量值。
[0123] 优选地，在胶囊内窥镜内部设有至少两个磁传感器，以通过多个磁传感器同时进行测量，来提高驱动磁体磁场测量的准确度。作为示例地，驱动磁体的磁场测量值可表示为Vl，其中，l表示第l个磁传感器。
[0124] 当胶囊内窥镜内部设有至少两个磁传感器时，优选地，磁传感器在胶囊内窥镜内部位置分散，磁传感器与胶囊内窥镜内部的磁体分离布置、且距离固定，单个磁传感器的磁场测量范围大于或等于胶囊内窥镜内部两个磁体的磁场强度叠加值，以提高磁传感器的利用率和测量效果。
[0125] 进一步优选地，磁传感器为三轴磁传感器，可以测量到三个测量轴方向上的磁场强度，以提高磁传感器的测量效果。
[0126] 自旋测量单元72，用于测量胶囊内窥镜的自旋角度，获得胶囊内窥镜的自旋角度的测量值。
[0127] 在本发明实施例中，在获得驱动磁体的磁场测量值后，由于胶囊内窥镜的位置为三维位置，即包括三个自由度，胶囊内窥镜的姿态为三维空间的姿态，也包括三个自由度，为了后续能够确定胶囊内窥镜的位姿信息(位置和姿态)，即为了可以确定6个自由度的值，需要测量胶囊内窥镜的自旋角度。可通过胶囊内窥镜中的自旋角度测量传感器实现对胶囊内窥镜自旋角度的测量。
[0128] 优选地，在胶囊内窥镜内部设有惯性传感器，通过惯性传感器来测量胶囊内窥镜的自旋角度，以提高胶囊内窥镜自旋角度的测量准确度。
[0129] 具体地，惯性传感器包括线加速度传感器和陀螺仪。
[0130] 当惯性传感器中的线加速度传感器为三轴线加速度传感器和三轴陀螺仪时，如图2所示，ax、ay、az分别表示三轴线加速度传感器的三个测量轴，ωx、ωy、ωz分别表示三轴陀螺仪的三个测量轴，az、ωz的方向为H0方向，acc为惯性传感器受到的线加速度，θ为胶囊内窥镜的自旋角度， 为az与acc之间的夹角。三轴线加速度传感器在三个测量轴上的分量分别为：
进而求得胶囊内窥镜的自旋角度的测量值为θ＝arctan(-accy/accx)。通过惯性传感器中的三轴陀螺仪来计算胶囊内窥镜的自旋角度的测量值的公式为 其中，N为
三轴陀螺仪测量到数据的个数，dt为三轴陀螺仪的数据采集间隔，ωzi为ωz测量轴上测量胶囊内窥镜自旋的角速率数据点。在采用惯性传感器测量胶囊内窥镜的自旋角度时，可单独采用三轴线加速度传感器或三轴陀螺仪，也可以结合两者一起测量，其中，线加速度传感器为三轴线加速度传感器，陀螺仪测量轴的数量不作限制。在计算得到自旋角度的测量值θ后，需计算关于自旋角度的测量值θ的坐标变换关系R(θ)，依据磁传感器的轴向与惯性传感器的轴向，R(θ)的表示形会有所变化，当磁传感器的轴向与惯性传感器的轴向相同时，即磁传感器的轴向Bx、By、Bz分别对应惯性传感器的轴向ax(或ωx)、ay(或ωy)、az(或wz)，可得到关于自旋角度的测量值θ的坐标变换关系R(θ)为：
[0131] 其中，R(θ)用于后续求解胶囊内窥镜六个自由度的位姿信息。
[0132] 相对位姿计算单元73，用于依据驱动磁体的磁场测量值、自旋角度的测量值、预设驱动磁场模型和预设优化算法，计算胶囊内窥镜相对驱动磁体的位姿信息。
[0133] 在本发明实施例中，预先构建驱动磁场模型，依据驱动磁场模型、驱动磁体的磁场测量值和自旋角度的测量值，来计算驱动磁体的磁场测量误差，通过优化算法对磁场测量误差进行最小化，可计算得到胶囊内窥镜相对于驱动磁体的位姿信息。其中，驱动磁场模型的构建和胶囊内窥镜相对于驱动磁体的位姿信息的计算(包括磁场测量误差的计算和最小化过程)由定位系统中的处理装置执行。
[0134] 优选地，在构建驱动磁场模型时，获取各磁传感器与胶囊内窥镜中心的距离(xl,yl,zl)T，获取驱动磁体的相对磁导率μr，磁偶极子模型在磁定位运算中具有较高的运算效率，很适合实时定位，因此再依据获得的这些参数和磁偶极子模型，构建驱动磁场模型，驱动磁场模型表示为：
[0135] 其中，胶囊内窥镜中心与驱动磁体中心的相对位置为(a,b,c)T，胶囊内窥镜中心与驱动磁体中心的相对姿态为H0＝(m,n,p)T，Pl表示第l个磁传感器与驱动磁体中心的距离且Pl＝(xl-a,yl-b,zl-c)T，Rl为Pl的模，存在约束条件m2+n2+p2＝1，μ0为真空磁导率，MT为表征磁体磁场强度的常数，Bl为驱动磁场的磁场模型值。求解胶囊内窥镜相对驱动磁体的位姿信息，即求解(a,b,c)T和H0＝(m,n,p)T。
[0136] 在依据驱动磁场模型、驱动磁体的磁场测量值和自旋角度的测量值，计算驱动磁体的磁场测量误差Err时，进一步优选地，磁场测量误差表示为：
[0137] 以提高磁场测量误差的计算准确度，并能够通过误差最小化过程计算得到胶囊内窥镜相对驱动磁体的位姿信息。在通过优化算法对磁场测量误差进行最小化时，进一步优选地，优选算法采用非线性最小二乘法-LM算法或智能算法-PSO算法，以提高磁场测量误差的最小化效果。
[0138] 胶囊位姿确定单元74，用于根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，确定胶囊内窥镜在世界坐标系的位姿信息。
[0139] 在本发明实施例中，驱动磁体在世界坐标系的位姿信息(位置和姿态)是已知的，在获得胶囊内窥镜相对驱动磁体的位姿信息，即可根据驱动磁体在世界坐标系的位姿信息和胶囊内窥镜相对驱动磁体的位姿信息，转换得到胶囊内窥镜在世界坐标系的位姿信息，完成对胶囊内窥镜的定位。
[0140] 优选地，在主机用户界面建立胶囊内窥镜模型，根据定位得到的胶囊内窥镜的位姿信息，刷新胶囊内窥镜模型的位姿信息，在接收到用户输入的下一时刻位姿信息时，可通过调整驱动磁体的位置姿态产生磁力，使得胶囊内窥镜运动到下一时刻位姿信息中的位置和姿态，从而实现胶囊内窥镜定位和运动的人机交互过程，提高用户体验。此外，当胶囊内窥镜运动到下一时刻位姿信息中的位置和姿态时，根据胶囊内窥镜实时反馈的位姿信息，对胶囊内窥镜的位姿进行不断调整，以平衡胶囊内窥镜的受力，使得胶囊内窥镜实现悬浮。
[0141] 优选地，如图8所示，磁场测量单元71包括：
[0142] 第一磁场值获取单元711，用于获取预先测量的第一磁场值，第一磁场值为在不受驱动磁体的影响下胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值；
[0143] 第二磁场值测量单元712，用于通过磁传感器测量得到第二磁场值，第二磁场值为驱动磁体、胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值；以及[0144] 磁场值计算单元713，用于依据第一磁场值和第二磁场值，计算驱动磁体的磁场测量值。
[0145] 在本发明实施例中，在没有驱动磁体的磁场时(即胶囊内窥镜位于驱动磁体的磁场范围之外时)，通过胶囊内窥镜内部的磁传感器测量第一磁场值，第一磁场值为胶囊内窥镜中的磁体、地磁场及周围环境磁场的磁场总值，周围环境磁场为胶囊内窥镜周围设备的磁场，不包括驱动磁体的磁场。其中，在不考虑周围环境磁场变化时，第一磁场值为固定值。
[0146] 在本发明实施例中，在驱动磁体驱动胶囊内窥镜移动时，胶囊内窥镜位于驱动磁体的磁场范围内，通过胶囊内窥镜内部的磁传感器测量第二磁场值，第二磁场值为驱动磁体、胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值。
[0147] 在本发明实施例中，第一磁场值为胶囊内窥镜中的磁体、地磁场及周围环境磁场的磁场总值，第二磁场值为驱动磁体、胶囊内窥镜内部磁体、地磁场及周围环境磁场的磁场总值，因此可通过将第二磁场值减去第一磁场值，计算得到驱动磁体的磁场测量值Vl。
[0148] 优选地，驱动磁体的磁场测量值Vl的计算公式为：
[0149] Vl＝Vl1-Vl0，其中，Vl0为第l个磁传感器测量到的第一磁场值，Vl1为第l个磁传感器测量到的第二磁场值。
[0150] 优选地，如图8所示，相对位姿计算单元73包括：
[0151] 误差计算单元731，用于依据驱动磁体模型、驱动磁体的磁场测量值和自旋角度的测量值，计算驱动磁体的磁场测量误差；以及
[0152] 误差最小化单元732，用于通过优化算法对驱动磁体的磁场测量误差进行最小化，获得胶囊内窥镜相对驱动磁体的位姿信息。
[0153] 在本发明实施例中，误差计算单元731和误差最小化单元732的详细内容已在相对位姿计算单元73进行描述，不再赘述。
[0154] 优选地，定位系统还包括：
[0155] 参数获取单元，用于获取磁传感器与胶囊内窥镜中心的距离，并获取驱动磁体的相对磁导率；以及
[0156] 磁场模型构建单元，用于根据磁偶极子模型、磁传感器与胶囊内窥镜中心的距离和驱动磁体的相对磁导率，构建驱动磁场模型。
[0157] 在本发明实施例中，通过测量驱动磁体的磁场，测量胶囊内窥镜的自旋角度，依据驱动磁场模型、驱动磁体的磁场测量值、胶囊内窥镜的自旋角度的测量值和优化算法，对驱动磁体的磁场测量误差进行最小化，以确定胶囊内窥镜的位姿信息，从而有效地避免驱动磁场对定位准确度的影响，提高胶囊内窥镜的定位准确度。
[0158] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
