磁控胶囊系统及其位姿标定表示方法与流程

1.本发明涉及医疗设备技术领域，尤其涉及一种磁控胶囊系统及其位姿标定表示方法。


背景技术：

2.体内设备定位技术，如无线胶囊内窥镜体、侵入式医疗器械等体内定位技术，受到越来越多的关注。磁控胶囊系统通过磁力驱动胶囊内窥镜在体内运动，目前对胶囊内窥镜的驱动，还需要通过有丰富经验的医师来完成。医师通过内置镜头拍摄消化道内壁的检查图像，确定胶囊内窥镜的位置和姿态朝向，再通过外部控制磁体驱动胶囊内窥镜继续运动到下一位置。
3.由于磁力的极度非线性、非均匀空间分布特点，消化道可形变环境以及摩擦力的影响，胶囊自旋导致难以依据图像判断真实方位，使得仅依靠图像视觉反馈信息，无法精准定量控制胶囊到达目标位置和目标姿态角度，而通过定位方法确认胶囊的位置和姿态角度后，由于定位系统和磁力驱动系统独立运行，定位系统仅能起到辅助确认的作用，胶囊的下一步运动仍需要医师凭借经验判断，且胶囊的姿态角度常采用欧拉角，但欧拉角并不能直观体现胶囊的姿态，不便于实际控制操作使用。所以，目前系统操作复杂，且不够直观、精准。


技术实现要素：

4.为解决上述现有技术问题中的至少其一，本发明的目的在于提供一种通过闭环的形式准确控制胶囊内窥镜运动的磁控胶囊系统及其位姿标定方法。
5.为实现上述发明目的，本发明一实施方式提供一种磁控胶囊系统的位姿标定表示方法，包括如下步骤：
6.获取所述控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；
7.获取所述胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和欧拉角；
8.建立世界坐标系；
9.将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；
10.根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息；
11.将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；
12.根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息；
13.表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；
14.表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。
15.作为本发明的进一步改进，还包括步骤：
16.表示所述控制磁体的位姿，所述控制磁体的位姿表示为[mx,my,mz,mh,mv]，其中，[mx,my,mz]为磁体世界坐标，[mh,mv]为磁体姿态信息；
[0017]
表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿表示为[cx,cy,cz,ch,cv,cs]，其中，[cx,cy,cz]为胶囊世界坐标，[ch,cv,cs]为胶囊姿态信息。
[0018]
作为本发明的进一步改进，其中，所述磁体局域坐标包括所述控制磁体在第一局域坐标系中的可移动范围坐标；
[0019]
所述步骤“建立世界坐标系”包括：
[0020]
根据所述可移动范围坐标，确定所述世界坐标系的原点。
[0021]
作为本发明的进一步改进，其中，将所述可移动范围坐标的中间点作为所述世界坐标系的原点。
[0022]
作为本发明的进一步改进，所述步骤“将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标”包括：
[0023]
计算所述磁体局域坐标相对于所述世界坐标系的原点在各个坐标轴方向的第一组偏移量；
[0024]
将所述磁体世界坐标的值设置为所述第一组偏移量。
[0025]
作为本发明的进一步改进，其中，所述步骤“将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标”包括：
[0026]
计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量；
[0027]
将所述胶囊局域坐标与所述第二组偏移量的差值作为所述胶囊世界坐标的值。
[0028]
作为本发明的进一步改进，其中，所述第二组偏移量包括x轴差值和y轴差值；
[0029]
所述步骤“计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量”包括：
[0030]
将所述胶囊内窥镜和所述控制磁体在所述世界坐标系的xy平面内的竖直投影重合；
[0031]
获取此时的所述胶囊内窥镜在所述第二局域坐标系中的第一对齐坐标、以及所述控制磁体在所述世界坐标系中的第二对齐坐标；
[0032]
计算所述第一对齐坐标与所述第二对齐坐标在x轴方向的所述x轴差值、以及在y轴方向的所述y轴差值。
[0033]
作为本发明的进一步改进，所述第二组偏移量还包括z轴差值；
[0034]
所述步骤“计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量”包括：
[0035]
获取所述磁控胶囊系统的硬件参数；
[0036]
根据所述硬件参数确定所述z轴差值。
[0037]
为实现上述发明目的之一，本发明一实施例提供了一种磁控胶囊系统，其包括控制磁体和胶囊内窥镜，还包括：
[0038]
第一获取模块，用于获取所述控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；
[0039]
第二获取模块，用于获取所述胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和
欧拉角；
[0040]
建模模块，用于建立世界坐标系；
[0041]
控制磁体位置修正模块，用于将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；
[0042]
控制磁体姿态修正模块，用于根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息；
[0043]
胶囊内窥镜位置修正模块，用于将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；
[0044]
胶囊内窥镜姿态修正模块，用于根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息；
[0045]
控制磁体表示模块，用于表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；
[0046]
胶囊内窥镜表示模块，用于表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。
[0047]
为实现上述发明目的之一，本发明一实施例提供了一种电子设备，包括：
[0048]
存储模块，存储计算机程序；
[0049]
处理模块，执行所述计算机程序时可实现上述的磁控胶囊系统的位姿标定表示方法中的步骤。
[0050]
为实现上述发明目的之一，本发明一实施例提供了一种可读存储介质，其存储有计算机程序，该计算机程序被处理模块执行时可实现上述的磁控胶囊系统的位姿标定表示方法中的步骤。
[0051]
与现有技术相比，本发明具有以下有益效果：将控制磁体和胶囊内窥镜对应的两个系统进行整合，统一、直观地表示整个磁控胶囊系统的状态，从而便于高效、精准地闭环控制胶囊内窥镜实现消化道检查，拓展胶囊内窥镜的控制方法及应用场景，提高医学辅助诊断的准确度和精确度。
[0052]
且通过将磁体世界坐标、磁体姿态信息、胶囊世界坐标和胶囊姿态信息在世界坐标系中进行统一标定，且将欧拉角转换为胶囊姿态信息，可以直观地展现胶囊的姿态，从而便于实际控制操作使用。控制磁体和胶囊内窥镜对应的两个系统对应关系确定，两个系统完成匹配，后面的工作无需重复进行坐标系的换算。
附图说明
[0053]
图1是本发明一实施例的位姿标定表示方法的流程图；
[0054]
图2是本发明一实施例的磁控胶囊系统应用于人体的结构示意图；
[0055]
图3是本发明一实施例的世界坐标系的建立方法的示意图；
[0056]
图4是本发明一实施例的控制磁体的姿态信息的计算方法的示意图；
[0057]
图5是本发明一实施例的第二组偏移量的计算方法的示意图；
[0058]
图6是本发明一实施例的胶囊内窥镜的欧拉角的示意图；
[0059]
图7是本发明一实施例的胶囊内窥镜姿的姿态信息的计算方法的示意图；
[0060]
图8是本发明一实施例的磁控胶囊系统的结构框图；
[0061]
图9是本发明一实施例的磁控胶囊系统的模块示意图；
[0062]
其中，1000、磁控胶囊系统；100、磁控系统；200、胶囊定位系统；201、胶囊内窥镜；300、床面；400、人体；10、控制磁体；20、信号传输模块；30、存储模块；40、处理模块；50、磁传感器；60、加速度传感器；70、信号输送模块；80、摄像模块；90、通信总线。
具体实施方式
[0063]
以下将结合附图所示的具体实施方式对本发明进行详细描述。但这些实施方式并不限制本发明，本领域的普通技术人员根据这些实施方式所做出的结构、方法、或功能上的变换均包含在本发明的保护范围内。
[0064]
本发明一实施例提供一种通过闭环的形式准确控制胶囊内窥镜运动的磁控胶囊系统及其位姿标定表示方法，磁控胶囊系统是应用于人体的设备，如无线胶囊内窥镜体、侵入式医疗器械等，用于定位体内的无线胶囊的位置。该方法将控制磁体和胶囊内窥镜统一到同一世界坐标系中，便于后续对无线胶囊的准确控制。
[0065]
本实施例的磁控胶囊系统1000，包括磁控系统100、胶囊定位系统200、控制磁体10和胶囊内窥镜201，磁控系统100用于通过控制胶囊内窥镜201的运动，胶囊定位系统200用于对胶囊内窥镜201定位，胶囊内窥镜201内部装有传感器模块，传感器模块包括用于检测磁场的磁传感器50(mag sensor)，如霍尔传感器、磁阻传感器(amr、gmr、tmr)等，控制磁体10包括用于发出磁场的磁源、用于控制磁源运动的伺服电机，胶囊内窥镜201内部也有磁性件，通过磁源对磁性件的作用力，实现磁控系统100对胶囊内窥镜201的位置和姿态的控制。
[0066]
图1为本技术一个实施方式的一种磁控胶囊系统1000的位姿标定表示方法，图2为本技术一个实施方式的磁控胶囊系统1000的结构图，胶囊内窥镜201位于人体400内部，人体400躺平于床面300上，人体400外部设有磁控系统100。在检查时，控制磁体10发出的磁场，控制人体400内的胶囊内窥镜201的运动。
[0067]
虽然本技术提供了如下述实施方式或流程图所述的方法操作步骤，但是基于常规或者无需创造性的劳动，所述方法在逻辑性上不存在必要因果关系的步骤中，这些步骤的执行顺序不限于本技术实施方式中所提供的执行顺序。
[0068]
具体的磁控胶囊系统1000的位姿标定表示方法，包括如下步骤：
[0069]
步骤101：获取控制磁体10在第一局域坐标系中的磁体局域坐标和朝向角度。
[0070]
所述磁体局域坐标可以包括控制磁体10在第一局域坐标系中的可移动范围坐标。
[0071]
磁体局域坐标和朝向角度可以通过磁控系统100的数据接口，由伺服电机的传动数据获得。
[0072]
第一局域坐标系是磁控系统100中的一个局域坐标，定义出该局域坐标中的原点，再根据伺服电机的驱动量，即可计算出控制磁体10的当前位置和朝向角度。
[0073]
在第一局域坐标系中，磁体局域坐标包括xyz轴三个方向的参数值：[mag
x
，magy，magz]，控制磁体10具有轴对称性，可以根据磁化方向n极的朝向角度确定出朝向角度，朝向角度可以取其方向的单位矢量，或者在后期再归一化后参与计算，这里控制磁体10的朝向角度取其单位矢量：[p
x
,py,pz]。
[0074]
另外，根据伺服电机的行程轨迹，可以计算出控制磁体10的各个方向上的可移动范围，这些范围的端点为控制磁体10的极限位置，这些极限位置可以分别标示为：{x:
[mag
xmin
,mag
xmax
],y:[mag
ymin
,mag
ymax
],z:[mag
zmin
,mag
zmax
]}。
[0075]
步骤102：获取胶囊内窥镜201在第二局域坐标系中的胶囊局域坐标和欧拉角；
[0076]
胶囊局域坐标即为胶囊内窥镜201包括xyz轴三个方向的参数值：[cap
x
,capy,capz]。欧拉角包括横滚角roll、航向角yaw和俯仰角pitch，反应了胶囊内窥镜201的姿态朝向，横滚角roll、航向角yaw、俯仰角pitch可以依次标示为[θ0,θ1,θ2]，胶囊内窥镜201的欧拉角可以参图6所示。
[0077]
所述第二局域坐标系是胶囊内窥镜201中的一个局域坐标，该局域坐标的标定依赖于胶囊定位系统200的标定，胶囊定位系统200一般固定在检查的床面300以下，其原点的高度位置相对固定，但是该局域坐标的xy水平面内则可能发生移动。
[0078]
胶囊局域坐标和欧拉角可以通过胶囊定位系统200的相关软件接口功能，获取胶囊内窥镜201的位置、姿态角度状态数据。位置数据精确到1mm，姿态角度数据为避免矩阵计算精度的损失，可以采用浮点数弧度表示。
[0079]
胶囊内窥镜201内部可以设置有三轴磁传感器50和三轴加速度传感器60，人体400外部设置若干组磁定位设备，与胶囊内窥镜201协同工作，计算出胶囊内窥镜201所在的位置和姿态。
[0080]
步骤103：建立世界坐标系。
[0081]
世界坐标系用于将磁控系统100的第一局域坐标系和胶囊定位系统200的第二局域坐标系统一，将上述的控制磁体10的磁体局域坐标和朝向角度、胶囊内窥镜201的胶囊局域坐标和欧拉角放入同一坐标系中。
[0082]
本实施例的磁控胶囊系统1000的位姿标定表示方法，标定过程是在磁控系统100和胶囊定位系统200组装时进行一次标定，使得两个系统的坐标系匹配到同一世界坐标系中。后期如果功能模块升级、定位系统挪动等导致世界坐标系的变更，可以用本实施例的方法进行再次标定校准。一次标定之后，将磁控系统100和定位系统的坐标系的对应关系确定，经过本实施例的算法换算，第一局域坐标系和第二局域坐标系就与世界坐标系匹配完成，形成了统一的世界坐标系。
[0083]
世界坐标系的建立，可以人为指定，也可以根据所述可移动范围坐标，确定所述世界坐标系的原点。
[0084]
具体地，是将所述可移动范围坐标的中间点作为所述世界坐标系的原点，参图3所示，具体地，原点[mag
x0
,mag
y0
,mag
z0
]可以用如下公式进行计算：
[0085][0086]
步骤104：将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；
[0087]
根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息。
[0088]
在上述的世界坐标系建立完成后，再计算所述磁体局域坐标相对于所述世界坐标系的原点在各个坐标轴方向的第一组偏移量，然后将所述磁体世界坐标的值设置为所述第一组偏移量。
[0089]
具体地，在标定世界坐标系的原点后，控制磁体10在世界坐标系的位置[mx,my,mz]表示为：
[0090][0091]
其中，z0为便于应用习惯而引入的常数修正高度，可以设置为z0＝0，也可以设为原点到检查床面300的高度，也就是说相当于以检查床面300为mz＝0的平面。
[0092]
举例来说，控制系统xyz的边界范围为{x:[20,550],y:[-30,450],z:[-80,220]}，则世界坐标系原点坐标mag0＝[285,210,70]，根据控制系统获取的当前位置点坐标原始数据mag＝[200,150,100]，标定后的位置状态[mx,my,mz]＝[-85,-60,30]。
[0093]
另外，参图3所示，根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体10在所述世界坐标系中的磁体姿态信息。
[0094]
控制磁体具有轴对称性，磁化方向n极的朝向角度[mh,mv]仅与世界坐标系坐标轴方向定义相关。控制磁体10在世界坐标系中的磁体姿态信息，是类似于球坐标角度表示方式。具体的数值方面，可以定义控制磁体10磁化方向矢量与z轴正向的夹角为垂直倾斜角mv(取值范围[0,+180]度)；定义控制磁体10磁化方向矢量在xy平面投影矢量与y轴正向的夹角为水平方位角mh(取值范围[-180,+180]度)，并按照顺时针方向增加，如y轴正向时mh＝0，x轴正向时mh＝90，y轴负向时mh＝
±
180，x轴负向时mh＝-90。
[0095]
具体地，
[0096][0097]
其中，[px,py,pz]为上述的控制磁体10在n极的磁化方向的单位矢量在xyz坐标轴的投影分量。
[0098]
可选地，控制磁体与控制机械固定连接，通过特殊角度位置的光电开关标定垂直、水平角度的零点，然后通过驱动控制磁体旋转的伺服电机相对转动状态直接等效换算，可以精准地获取角度状态数据，无需通过现场测量控制磁体磁场方向确定上述姿态角度。
[0099]
步骤105：将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；
[0100]
根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息。
[0101]
计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量，所述第二组偏移量包括x轴差值、y轴差值和z轴差值；
[0102]
将所述胶囊局域坐标与所述第二组偏移量的差值作为所述胶囊世界坐标的值。
[0103]
第二组偏移量的计算方式，可以参图5所示。具体地，将所述胶囊内窥镜201和所述控制磁体10在所述世界坐标系的xy平面内的竖直投影重合，将控制磁体10适当拉高以减弱对胶囊内窥镜201的吸力，避免控制磁体10移动时导致胶囊内窥镜201的平移或滚动，并旋转控制磁体到磁化方向竖直朝上，即n极竖直向上，再将胶囊内窥镜201移动至控制磁体下
方附近，控制磁体10在xy平面内移动，通过负反馈修正进行调整，使得胶囊内窥镜201达竖直朝上状态，此时，控制磁体10与胶囊内窥镜201在xy平面内的竖直投影完全重合，完成胶囊定位系统的xy平面标定对齐。
[0104]
获取此时的所述胶囊内窥镜201在所述第二局域坐标系中的第一对齐坐标[cap
xu
,cap
yu
]、以及所述控制磁体10在所述世界坐标系中的第二对齐坐标[m
xu
,m
yu
]，下标u是指的控制磁体移动到了胶囊正上方，此时，胶囊位于控制磁体正下方，用下标u(under)标记。
[0105]
计算所述第一对齐坐标[cap
xu
,cap
yu
]与所述第二对齐坐标[m
xu
,m
yu
]在x轴方向的所述x轴差值cap
x0
、以及在y轴方向的所述y轴差值cap
y0
。
[0106]
cap
x0
和cap
y0
也就是第二局域坐标系与世界坐标系在x轴和y轴上的坐标值的偏差，这样将两者统一到一个坐标系内进行比较。
[0107]
第二组偏移量的计算，是在系统运行之初的设定，在该第二组偏移量的值计算后，在后续使用时，不需要再将控制磁体10移动至胶囊内窥镜201的上方，该cap
x0
和cap
y0
在后续的使用中不会再次计算。胶囊内窥镜201的位置可以是任意位置，胶囊内窥镜201在第二局域坐标系中的胶囊局域坐标为上述的[cap
x
,capy,capz]。
[0108]
另外，z轴差值的计算步骤如下：
[0109]
获取所述磁控胶囊系统1000的硬件参数；
[0110]
根据所述硬件参数确定所述z轴差值。
[0111]
胶囊内窥镜201在世界坐标系中的z轴标定至关重要，涉及胶囊重力、胶囊浮力、摩擦力、磁体吸力及力矩的平衡。控制磁体10与胶囊内窥镜201的z方向距离直接决定了磁力吸力的大小，并且磁力与距离r满足极度非线性关系，距离的远近影响胶囊内窥镜201的受力平衡，导致胶囊内窥镜201出现沉底、水面悬浮、吸顶等不同情境状态切换。准确的z轴标定，便于实时获取胶囊内窥镜201的高度距离，为后续的控制动作提供数据的依据。
[0112]
第二局域坐标系的原点标定依赖于控制系统和胶囊定位系统200的硬件设定，所以其具体数值参考具体设备的参数确定。
[0113]
以第一局域坐标系的原点在床面300的上方，第二局域坐标系的原点在床面300的下方为例，第一局域坐标系的原点距离检查床面300的高度差与检查床面300距离第二局域坐标系的原点的高度差之和，作为所述z轴差值。
[0114]
具体地，第二组偏移量的计算公式为：
[0115][0116]
其中，z1为第一局域坐标系的原点距离检查床面300的高度差，z2为检查床面300距离第二局域坐标系的原点的高度差。
[0117]
结合上述的步骤“将所述胶囊局域坐标与所述第二组偏移量的差值作为所述胶囊世界坐标的值”，胶囊世界坐标[cx,cy,cz]的计算公式为：
[0118]
[0119]
另外，参图7所示，根据所述欧拉角确定所述胶囊内窥镜201在所述世界坐标系中的投影，计算所述胶囊内窥镜201在所述世界坐标系中的胶囊姿态信息。将欧拉角转换为胶囊姿态信息，可以直观地展现胶囊的姿态，从而便于实际控制操作使用。
[0120]
具体地，胶囊内窥镜在世界坐标系中的胶囊姿态信息，是类似于球坐标角度表示方式。具体的数值方面，可以采用类似于磁体姿态信息的定义方式，将欧拉角转换为朝向姿态角[ch,cv]、以及胶囊自旋角度cs(可以对与镜头正方向定义相关的固定相位差修正c
s0
)。
[0121]
也就是说，胶囊内窥镜201的胶囊姿态信息使用[ch,cv,cs]表示，其中：h表示水平方位角，v表示垂直倾斜角,s表示胶囊自旋角，参图7所示。
[0122]
定义胶囊内窥镜201的头部朝向与z轴正向的夹角为胶囊垂直倾斜角cv(取值范围[0,+180]度)；定义胶囊内窥镜201的头部朝向在xy平面投影矢量与y轴正向的夹角为胶囊水平方位角ch(取值范围[-180,+180]度)，并按照顺时针方向增加(y轴正向时ch＝0，x轴正向时ch＝90，y轴负向时ch＝
±
180，x轴负向时ch＝-90)；胶囊自旋角为所述胶囊内窥镜的镜头的朝向角度，定义胶囊内窥镜201的镜头拍摄图像正立时胶囊自旋角cs＝0，按照顺时针方向增加，胶囊内窥镜201的姿态信息[ch,cv,cs]的计算公式为：
[0123][0124]
其中，第二局域坐标系z轴在世界坐标系的投影为：
[0125]
p＝r
·
[0 0 1]
t
＝[p
x p
y pz]
t
[0126]
胶囊内窥镜201按照z(roll),y(pitch),x(yaw)的顺序旋转的方向余弦矩阵表示为：
[0127][0128]
z(roll),y(pitch),x(yaw)旋转矩阵分别记为：
[0129][0130]
其中，ck≡cos(θk)，sk≡sin(θk)，θk为上述的相应的欧拉角，k＝0,1,2。
[0131]
公式1中r的具体值，在公式2中查询。例如公式1中的r
20
，在公式2中查询为c0s1c2+s0s2。
[0132]
步骤106：表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；
[0133]
经过步骤104，可以将控制磁体10在世界坐标系的位姿表示为：
[0134]
[mx,my,mz,mh,mv]，其中，[mx,my,mz]为磁体世界坐标，[mh,mv]为磁体姿态信息。
[0135]
步骤107：表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。
[0136]
经过步骤105，可以将胶囊内窥镜201在世界坐标系的位姿表示为：
[0137]
[cx,cy,cz,ch,cv,cs]，其中，[cx,cy,cz]为胶囊世界坐标，[ch,cv,cs]为胶囊姿态信息。
[0138]
本实施例的磁控胶囊系统1000将控制磁体10和胶囊内窥镜201统一到同一世界坐标系中，且将欧拉角转换为胶囊姿态信息，可以直观地展现胶囊的姿态，继而可以直观地表示整个磁控胶囊系统1000的状态，为后续高效、精准地闭环控制胶囊内窥镜201实现消化道检查提供便利，拓展胶囊内窥镜201的控制方法及应用场景，提高医学辅助诊断的准确度和精确度。
[0139]
在一个实施例中，提供了一种磁控胶囊系统1000，如图8所示。除了控制磁体和胶囊内窥镜，该磁控胶囊系统1000还可以包括：
[0140]
第一获取模块，用于获取所述控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；
[0141]
第二获取模块，用于获取所述胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和欧拉角；
[0142]
建模模块，用于建立世界坐标系；
[0143]
控制磁体位置修正模块，用于将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；
[0144]
控制磁体姿态修正模块，用于根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息；
[0145]
胶囊内窥镜位置修正模块，用于将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；
[0146]
胶囊内窥镜姿态修正模块，用于根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息；
[0147]
控制磁体表示模块，用于表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；
[0148]
胶囊内窥镜表示模块，用于表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。
[0149]
在一个实施例中，控制磁体表示模块将所述控制磁体的位姿表示为[mx,my,mz,mh,mv]，其中，[mx,my,mz]为磁体世界坐标，[mh,mv]为磁体姿态信息
[0150]
在一个实施例中，胶囊内窥镜表示模块将所述胶囊内窥镜的位姿表示为[cx,cy,cz,ch,cv,cs]，其中，[cx,cy,cz]为胶囊世界坐标，[ch,cv,cs]为胶囊姿态信息
[0151]
在一个实施例中，建模模块根据所述可移动范围坐标，确定所述世界坐标系的原点。
[0152]
在一个实施例中，建模模块将所述可移动范围坐标的中间点作为所述世界坐标系的原点。
[0153]
在一个实施例中，控制磁体位置修正模块计算所述磁体局域坐标相对于所述世界坐标系的原点在各个坐标轴方向的第一组偏移量，并将所述磁体世界坐标的值设置为所述
第一组偏移量。
[0154]
在一个实施例中，胶囊内窥镜位置修正模块计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量；并将所述胶囊局域坐标与所述第二组偏移量的差值作为所述胶囊世界坐标的值。
[0155]
在一个实施例中，胶囊内窥镜姿态修正模块将所述胶囊内窥镜201和所述控制磁体10在所述世界坐标系的xy平面内的竖直投影重合；
[0156]
第二获取模块获取此时的所述胶囊内窥镜201在所述第二局域坐标系中的第一对齐坐标，第一获取模块获取所述控制磁体10在所述世界坐标系中的第二对齐坐标；
[0157]
胶囊内窥镜位置修正模块计算所述第一对齐坐标与所述第二对齐坐标在x轴方向的所述x轴差值、以及在y轴方向的所述y轴差值。
[0158]
在一个实施例中，磁控胶囊系统1000还包括数据接口，通过数据接口获取所述磁控胶囊系统1000的硬件参数；
[0159]
胶囊内窥镜坐标修正模块根据所述硬件参数确定所述z轴差值。
[0160]
所述磁控胶囊系统1000还可以包括计算机、笔记本、掌上电脑及云端服务器等计算设备。进一步可包括，但不仅限于，处理模块40、存储模块30。本领域技术人员可以理解，所述示意图仅仅是磁控胶囊系统1000的示例，并不构成对磁控胶囊系统1000终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述磁控胶囊系统1000还可以包括输入输出设备、网络接入设备、总线等。
[0161]
需要说明的是，本发明实施例的磁控胶囊系统1000中未披露的细节，请参照本发明实施例的磁控胶囊系统1000的位姿标定表示方法中所披露的细节。
[0162]
根据本发明的磁控胶囊系统1000，控制磁体坐标修正模块和胶囊内窥镜坐标修正模块将控制磁体10和胶囊内窥镜201统一到同一世界坐标系中，继而可以直观地表示整个磁控胶囊系统1000的状态，为后续高效、精准地闭环控制胶囊内窥镜201实现消化道检查提供便利，拓展胶囊内窥镜201的控制方法及应用场景，提高医学辅助诊断的准确度和精确度。
[0163]
如图9所示，是本发明一实施例提供的磁控胶囊系统1000的模块示意图。磁控胶囊系统1000还包括上述的磁控系统100、胶囊定位系统200、控制磁体10和胶囊内窥镜201、处理模块40、存储模块30、胶囊内窥镜201内的各模块、以及存储在所述存储模块30中并可在所述处理模块40上运行的计算机程序，例如上述的位姿标定表示方法程序。所述处理模块40执行所述计算机程序时实现上述各个位姿标定方法实施例中的步骤，例如图1所示的步骤。
[0164]
控制磁体10的磁源通过伺服电机及传动机构控制驱动其运动至指定位置，通过磁控系统100的数据接口，获取伺服电机的传动数据，经过固定的比例转换公式，获取控制磁体10的位置、姿态角度状态原始数据。位置数据精确到1mm，角度数据精确到1度。
[0165]
控制磁体10与传动机构固定连接，通过在一些位置的光电开关，如一些特殊垂直、水平角度标定零点，然后通过对驱动控制磁体10运动的伺服电机的驱动量的换算，可以精准地驱动控制磁体10在世界坐标系中运动到目标位置，无需通过现场测量控制磁体10磁场方向确定其姿态角度。
[0166]
胶囊内窥镜201可以包括磁场传感器、加速度传感器60、信号输送模块70、磁性件
和摄像模块80，磁场传感器、加速度传感器60、磁性件如上文所述，可以通过内部三轴磁传感器50、三轴加速度传感器60、imu传感器和外部多组磁定位设备协同工作，计算出胶囊内窥镜201所在的位置和姿态，通过控制磁体10对磁性件的作用驱动胶囊内窥镜201运动。信号输送模块70将信息传输至外界的处理模块40或服务器中，外界驱动无线胶囊运动到指定位置后，摄像模块80拍摄人体400内的照片通过信号输出模块传输至外界，完成对体内的拍摄。
[0167]
控制磁体10可以适当拉高以减弱对胶囊的吸力，或者降低以增大对胶囊的吸力，控制胶囊内窥镜201在沉底、水面悬浮、吸顶等不同情境状态中切换。
[0168]
磁控胶囊系统1000还可以包括信号传输模块20和通信总线90。信号传输模块20用于将数据发送至处理模块40或服务器，信号输送模块70和信号传输模块20可以通过无线连接的形式传输数据，如蓝牙、wifi、zigbee等，通信总线90用于将控制磁体10、信号传输模块20、处理模块40与存储模块30之间建立连接，通信总线90可包括一通路，在上述的控制磁体10、信号传输模块20、处理模块40与存储模块30之间传送信息。
[0169]
另外，本发明还提出了一种电子设备，其包括存储模块30和处理模块40，处理模块40执行所述计算机程序时可实现上述的位姿标定表示方法中的步骤，也就是说，实现上述位姿标定方法中的任意一个技术方案中的步骤。
[0170]
该电子设备可以是集成于磁控胶囊系统1000内的一部分、或者是本地的终端设备、还可以是云端服务器的一部分。
[0171]
所述处理模块40可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。所述处理模块40是所述磁控胶囊系统1000的控制中心，利用各种接口和线路连接整个磁控胶囊系统1000的各个部分。
[0172]
所述存储模块30可用于存储所述计算机程序和/或模块，所述处理模块40通过运行或执行存储在所述存储模块30内的计算机程序和/或模块，以及调用存储在存储模块30内的数据，实现所述磁控胶囊系统1000的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital,sd)卡，闪存卡(flash card)、至少—个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0173]
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储模块30中，并由所述处理模块40执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述磁控胶囊系统的位姿标定表示方法中的执行过程。
[0174]
进一步地，本发明一实施例提供了一种可读存储介质，其存储有计算机程序，该计
算机程序被处理模块40执行时可实现上述的磁控胶囊系统的位姿标定表示方法中的步骤，也就是说，实现上述磁控胶囊系统的位姿标定表示方法中的任意一个技术方案中的步骤。
[0175]
所述磁控胶囊系统1000集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。
[0176]
其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、∪盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
[0177]
应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施方式中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
[0178]
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更均应包含在本发明的保护范围之内。

技术特征：
1.一种磁控胶囊系统的位姿标定表示方法，所述磁控胶囊系统包括控制磁体和胶囊内窥镜，其特征在于，包括如下步骤：获取所述控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；获取所述胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和欧拉角；建立世界坐标系；将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息；将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息；表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。2.根据权利要求1所述的位姿标定表示方法，其特征在于，所述磁体姿态信息包括水平方位角和垂直倾斜角，其中，所述水平方位角为所述控制磁体的磁化方向矢量在所述世界坐标系的xy平面的投影矢量与y轴正向的夹角，所述垂直倾斜角为所述控制磁体的磁化方向矢量与所述世界坐标系的z轴正向的夹角。3.根据权利要求2所述的位姿标定表示方法，其特征在于，还包括步骤：表示所述控制磁体的位姿，所述控制磁体的位姿表示为[mx,my,mz,mh,mv]，其中，[mx,my,mz]为所述控制磁体在所述世界坐标系中的位置，mh为所述水平方位角，mv为所述垂直倾斜角。4.根据权利要求1所述的位姿标定表示方法，其特征在于，所述胶囊姿态信息包括水平方位角、垂直倾斜角和胶囊自旋角，其中，所述水平方位角为所述胶囊内窥镜的头部朝向在所述世界坐标系的xy平面投影矢量与y轴正向的夹角，所述垂直倾斜角为所述胶囊内窥镜的头部朝向与所述世界坐标系的z轴正向的夹角，所述胶囊自旋角为所述胶囊内窥镜的镜头的朝向角度。5.根据权利要求4所述的位姿标定表示方法，其特征在于，还包括步骤：表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿表示为[cx,cy,cz,ch,cv,cs]，其中，[cx,cy,cz]为所述胶囊内窥镜在所述世界坐标系中的位置，ch为所述水平方位角，cv为所述垂直倾斜角，cs为所述胶囊自旋角。6.根据权利要求1所述的位姿标定表示方法，其特征在于，其中，所述磁体局域坐标包括所述控制磁体在第一局域坐标系中的可移动范围坐标；所述步骤“建立世界坐标系”包括：根据所述可移动范围坐标，确定所述世界坐标系的原点。7.根据权利要求6所述的位姿标定表示方法，其特征在于，其中，将所述可移动范围坐标的中间点作为所述世界坐标系的原点。8.根据权利要求6所述的位姿标定表示方法，其特征在于，所述步骤“将所述磁体局域
坐标修正为所述世界坐标系中的磁体世界坐标”包括：计算所述磁体局域坐标相对于所述世界坐标系的原点在各个坐标轴方向的第一组偏移量；将所述磁体世界坐标的值设置为所述第一组偏移量。9.根据权利要求1所述的位姿标定表示方法，其特征在于，其中，所述步骤“将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标”包括：计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量；将所述胶囊局域坐标与所述第二组偏移量的差值作为所述胶囊世界坐标的值。10.根据权利要求9所述的位姿标定表示方法，其特征在于，其中，所述第二组偏移量包括x轴差值和y轴差值；所述步骤“计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量”包括：将所述胶囊内窥镜和所述控制磁体在所述世界坐标系的xy平面内的竖直投影重合；获取此时的所述胶囊内窥镜在所述第二局域坐标系中的第一对齐坐标、以及所述控制磁体在所述世界坐标系中的第二对齐坐标；计算所述第一对齐坐标与所述第二对齐坐标在x轴方向的所述x轴差值、以及在y轴方向的所述y轴差值。11.根据权利要求10所述的位姿标定表示方法，其特征在于，所述第二组偏移量还包括z轴差值；所述步骤“计算所述第二局域坐标系的原点相对于所述世界坐标系的原点的第二组偏移量”包括：获取所述磁控胶囊系统的硬件参数；根据所述硬件参数确定所述z轴差值。12.一种磁控胶囊系统，其包括控制磁体和胶囊内窥镜，其特征在于，还包括：第一获取模块，用于获取所述控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；第二获取模块，用于获取所述胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和欧拉角；建模模块，用于建立世界坐标系；控制磁体位置修正模块，用于将所述磁体局域坐标修正为所述世界坐标系中的磁体世界坐标；控制磁体姿态修正模块，用于根据所述朝向角度在所述世界坐标系中的投影，计算所述控制磁体在所述世界坐标系中的磁体姿态信息；胶囊内窥镜位置修正模块，用于将所述胶囊局域坐标修正为所述世界坐标系中的胶囊世界坐标；胶囊内窥镜姿态修正模块，用于根据所述欧拉角确定所述胶囊内窥镜在所述世界坐标系中的投影，计算所述胶囊内窥镜在所述世界坐标系中的胶囊姿态信息；控制磁体表示模块，用于表示所述控制磁体的位姿，所述控制磁体的位姿包括磁体世界坐标和/或磁体姿态信息；
胶囊内窥镜表示模块，用于表示所述胶囊内窥镜的位姿，所述胶囊内窥镜的位姿包括胶囊世界坐标和/或胶囊姿态信息。13.一种电子设备，其特征在于，包括：存储模块，存储计算机程序；处理模块，执行所述计算机程序时可实现权利要求1至11中任意一项所述的磁控胶囊系统的位姿标定表示方法中的步骤。14.一种可读存储介质，其存储有计算机程序，其特征在于，该计算机程序被处理模块执行时可实现权利要求1至11中任意一项所述的磁控胶囊系统的位姿标定表示方法中的步骤。

技术总结
本发明揭示了一种磁控胶囊系统及其位姿标定表示方法，方法包括步骤：获取控制磁体在第一局域坐标系中的磁体局域坐标和朝向角度；获取胶囊内窥镜在第二局域坐标系中的胶囊局域坐标和欧拉角；建立世界坐标系；将上述参数在世界坐标系中修正；将控制磁体和胶囊内窥镜的位姿同时表示出来。将控制磁体和胶囊内窥镜统一到同一世界坐标系中，继而可以直观地表示整个磁控胶囊系统的状态，为后续高效、精准地闭环控制胶囊内窥镜实现消化道检查提供便利，拓展胶囊内窥镜的控制方法及应用场景，提高医学辅助诊断的准确度和精确度。学辅助诊断的准确度和精确度。学辅助诊断的准确度和精确度。


技术研发人员：黄志威 张行 袁文金 杨戴天杙 张皓
受保护的技术使用者：安翰科技（武汉）股份有限公司
技术研发日：2022.03.18
技术公布日：2023/9/22