基于三维线圈的胶囊机器人姿态测定系统及方法与流程

本发明涉及胶囊机器人姿态测定技术，具体是一种基于三维线圈的胶囊机器人姿态测定系统及方法。

背景技术：

胶囊机器人是一种可经口腔或肛门进入人体肠道的微型机器人，其可在肠道中自主运动，完成对肠道内腔的无创诊查。在诊查过程中，胶囊机器人的姿态会随着所处肠道环境的变化而发生随机变化，导致其无法对病灶进行定位。为此，需要对胶囊机器人的姿态进行测定，以实现对病灶的定位。在现有技术条件下，胶囊机器人的姿态测定主要采用九轴姿态传感器(由三轴加速度计、三轴陀螺仪、三轴磁强计组成)进行。然而在实际应用中，现有胶囊机器人姿态测定技术由于自身原理所限，存在如下问题：其一，胶囊机器人采用无线电能传输系统(由无线电能发射线圈和无线电能接收线圈组成)进行供电。在无线电能传输系统中，无线电能的传输媒介为交变磁场，交变磁场会对九轴姿态传感器中的三轴磁强计造成严重的磁干扰，由此导致三轴磁强计的测量结果不准确，从而导致姿态测定结果不准确。其二，现有胶囊机器人姿态测定技术在对胶囊机器人的姿态角进行解算时，存在运算量大的问题，因此其不利于实现姿态的实时测定。基于此，有必要发明一种基于三维线圈的胶囊机器人姿态测定系统及方法，以解决现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题。

技术实现要素：

本发明为了解决现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题，提供了一种基于三维线圈的胶囊机器人姿态测定系统及方法。

本发明是采用如下技术方案实现的：

基于三维线圈的胶囊机器人姿态测定系统，包括置于体外的无线电能发射线圈a、置于体外的无线电能发射线圈b、安装于胶囊机器人上的三维无线电能接收线圈、安装于胶囊机器人上的机载电路、置于体外的无线信号接收电路、置于体外的上位机；

无线电能发射线圈a、无线电能发射线圈b均为亥姆霍兹线圈，且二者相互正交布置；

所述三维无线电能接收线圈包括线圈a、线圈b、线圈c；

线圈a、线圈b均为矩形线圈；线圈c为圆形线圈；线圈a、线圈b、线圈c两两相互正交布置，且线圈c的中心轴线与胶囊机器人的中心轴线重合；

所述机载电路包括取样电路a、取样电路b、取样电路c、adc模块、主控电路、无线信号发射电路、整流电路a、整流电路b、整流电路c、稳压电路；

取样电路a的输入端与线圈a的一端连接；取样电路b的输入端与线圈b的一端连接；取样电路c的输入端与线圈c的一端连接；取样电路a的输出端、取样电路b的输出端、取样电路c的输出端均与adc模块的输入端连接；adc模块的输出端与主控电路的输入端连接；主控电路的输出端与无线信号发射电路的输入端连接；

整流电路a的两个输入端分别与线圈a的两端连接；整流电路b的两个输入端分别与线圈b的两端连接；整流电路c的两个输入端分别与线圈c的两端连接；整流电路a的正输出端与稳压电路的正输入端连接；整流电路a的负输出端与整流电路b的正输出端连接；整流电路b的负输出端与整流电路c的正输出端连接；整流电路c的负输出端与稳压电路的负输入端连接；稳压电路的输出端分别与adc模块的电源端、主控电路的电源端、无线信号发射电路的电源端连接；

无线信号发射电路的输出端与无线信号接收电路的输入端无线连接；无线信号接收电路的输出端与上位机的输入端连接。

所述三维无线电能接收线圈通过磁芯安装于胶囊机器人上。

所述取样电路a包括整流二极管、第一分压电阻、第二分压电阻；整流二极管的阳极作为取样电路a的输入端；第一分压电阻的一端与整流二极管的阴极连接，另一端作为取样电路a的输出端；第二分压电阻的一端通过第一分压电阻与整流二极管的阴极连接，另一端接地；所述取样电路b的结构、取样电路c的结构均与取样电路a的结构一致。

所述第一分压电阻的阻值、第二分压电阻的阻值均大于10kω。

所述整流电路a、整流电路b、整流电路c均为全波整流电路。

所述稳压电路为线性低压差稳压芯片。

基于三维线圈的胶囊机器人姿态测定方法(该方法是基于本发明所述的基于三维线圈的胶囊机器人姿态测定系统实现的)，该方法是采用如下步骤实现的：

步骤一：无线电能发射线圈b不工作，无线电能发射线圈a开始工作；此时，无线电能发射线圈a激发出交变磁场，线圈a、线圈b、线圈c在交变磁场的作用下各自产生一路感应电压；三路感应电压先分别经取样电路a、取样电路b、取样电路c进行取样，再经adc模块进行ad转换，然后依次经主控电路、无线信号发射电路、无线信号接收电路发送至上位机；在此过程中，三路感应电压先分别经整流电路a、整流电路b、整流电路c进行整流，再进行串联叠加，然后经稳压电路进行稳压，由此向adc模块、主控电路、无线信号发射电路提供稳定的工作电压；

步骤二：无线电能发射线圈a停止工作，无线电能发射线圈b开始工作；此时，无线电能发射线圈b激发出交变磁场，线圈a、线圈b、线圈c在交变磁场的作用下各自产生一路感应电压；三路感应电压先分别经取样电路a、取样电路b、取样电路c进行取样，再经adc模块进行ad转换，然后依次经主控电路、无线信号发射电路、无线信号接收电路发送至上位机；在此过程中，三路感应电压先分别经整流电路a、整流电路b、整流电路c进行整流，再进行串联叠加，然后经稳压电路进行稳压，由此向adc模块、主控电路、无线信号发射电路提供稳定的工作电压；

步骤三：上位机根据感应电压、交变磁场的参数、三维无线电能接收线圈的参数实时解算出胶囊机器人的横滚角α、俯仰角β、航向角γ，并将解算结果进行实时显示和存储，由此对胶囊机器人的姿态进行实时测定；具体解算公式如下：

式(1)～(6)中：εaa、εba、εca分别表示无线电能发射线圈a工作时线圈a、线圈b、线圈c产生的感应电压的幅值；εab、εbb、εcb分别表示无线电能发射线圈b工作时线圈a、线圈b、线圈c产生的感应电压的幅值；ωa分别表示无线电能发射线圈a激发出的交变磁场的幅值、频率；ωb分别表示无线电能发射线圈b激发出的交变磁场的幅值、频率；na、sa、μa分别表示线圈a的匝数、单匝围成面积、围成区域的有效磁导率；nb、sb、μb分别表示线圈b的匝数、单匝围成面积、围成区域的有效磁导率；nc、sc、μc分别表示线圈c的匝数、单匝围成面积、围成区域的有效磁导率；ωa、ωb、na、sa、μa、nb、sb、μb、nc、sc、μc均为已知量；

步骤四：重复执行步骤一至步骤三，由此持续对胶囊机器人的姿态进行实时测定。

与现有胶囊机器人姿态测定技术相比，本发明所述的基于三维线圈的胶囊机器人姿态测定系统及方法具备了如下优点：其一，本发明不再采用九轴姿态传感器，而是直接利用三维无线电能接收线圈的感应电压实现了胶囊机器人的姿态测定，因此其不会被无线电能传输系统的交变磁场干扰，从而使得姿态测定结果更准确。其二，本发明仅仅利用简单的三角函数运算，即可实现胶囊机器人的姿态角解算，因此其运算量更小，从而有利于实现姿态的实时测定。

本发明有效解决了现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题，适用于胶囊机器人的姿态测定。

附图说明

图1是本发明中基于三维线圈的胶囊机器人姿态测定系统的结构示意图。

图2是本发明中无线电能发射线圈a和无线电能发射线圈b的单匝模型示意图。

图3是本发明中三维无线电能接收线圈的单匝模型示意图。

图4是本发明中三维无线电能接收线圈和磁芯的结构示意图。

图5是本发明中三维无线电能接收线圈和磁芯在胶囊机器人上的安装示意图。

具体实施方式

基于三维线圈的胶囊机器人姿态测定系统，包括置于体外的无线电能发射线圈a、置于体外的无线电能发射线圈b、安装于胶囊机器人上的三维无线电能接收线圈、安装于胶囊机器人上的机载电路、置于体外的无线信号接收电路、置于体外的上位机；

无线电能发射线圈a、无线电能发射线圈b均为亥姆霍兹线圈，且二者相互正交布置；

所述三维无线电能接收线圈包括线圈a、线圈b、线圈c；

线圈a、线圈b均为矩形线圈；线圈c为圆形线圈；线圈a、线圈b、线圈c两两相互正交布置，且线圈c的中心轴线与胶囊机器人的中心轴线重合；

所述机载电路包括取样电路a、取样电路b、取样电路c、adc模块、主控电路、无线信号发射电路、整流电路a、整流电路b、整流电路c、稳压电路；

取样电路a的输入端与线圈a的一端连接；取样电路b的输入端与线圈b的一端连接；取样电路c的输入端与线圈c的一端连接；取样电路a的输出端、取样电路b的输出端、取样电路c的输出端均与adc模块的输入端连接；adc模块的输出端与主控电路的输入端连接；主控电路的输出端与无线信号发射电路的输入端连接；

整流电路a的两个输入端分别与线圈a的两端连接；整流电路b的两个输入端分别与线圈b的两端连接；整流电路c的两个输入端分别与线圈c的两端连接；整流电路a的正输出端与稳压电路的正输入端连接；整流电路a的负输出端与整流电路b的正输出端连接；整流电路b的负输出端与整流电路c的正输出端连接；整流电路c的负输出端与稳压电路的负输入端连接；稳压电路的输出端分别与adc模块的电源端、主控电路的电源端、无线信号发射电路的电源端连接；

无线信号发射电路的输出端与无线信号接收电路的输入端无线连接；无线信号接收电路的输出端与上位机的输入端连接。

所述三维无线电能接收线圈通过磁芯安装于胶囊机器人上。

所述取样电路a包括整流二极管、第一分压电阻、第二分压电阻；整流二极管的阳极作为取样电路a的输入端；第一分压电阻的一端与整流二极管的阴极连接，另一端作为取样电路a的输出端；第二分压电阻的一端通过第一分压电阻与整流二极管的阴极连接，另一端接地；所述取样电路b的结构、取样电路c的结构均与取样电路a的结构一致。

所述第一分压电阻的阻值、第二分压电阻的阻值均大于10kω。

所述整流电路a、整流电路b、整流电路c均为全波整流电路。

所述稳压电路为线性低压差稳压芯片。

基于三维线圈的胶囊机器人姿态测定方法(该方法是基于本发明所述的基于三维线圈的胶囊机器人姿态测定系统实现的)，该方法是采用如下步骤实现的：

步骤一：无线电能发射线圈b不工作，无线电能发射线圈a开始工作；此时，无线电能发射线圈a激发出交变磁场，线圈a、线圈b、线圈c在交变磁场的作用下各自产生一路感应电压；三路感应电压先分别经取样电路a、取样电路b、取样电路c进行取样，再经adc模块进行ad转换，然后依次经主控电路、无线信号发射电路、无线信号接收电路发送至上位机；在此过程中，三路感应电压先分别经整流电路a、整流电路b、整流电路c进行整流，再进行串联叠加，然后经稳压电路进行稳压，由此向adc模块、主控电路、无线信号发射电路提供稳定的工作电压；

步骤二：无线电能发射线圈a停止工作，无线电能发射线圈b开始工作；此时，无线电能发射线圈b激发出交变磁场，线圈a、线圈b、线圈c在交变磁场的作用下各自产生一路感应电压；三路感应电压先分别经取样电路a、取样电路b、取样电路c进行取样，再经adc模块进行ad转换，然后依次经主控电路、无线信号发射电路、无线信号接收电路发送至上位机；在此过程中，三路感应电压先分别经整流电路a、整流电路b、整流电路c进行整流，再进行串联叠加，然后经稳压电路进行稳压，由此向adc模块、主控电路、无线信号发射电路提供稳定的工作电压；

步骤三：上位机根据感应电压、交变磁场的参数、三维无线电能接收线圈的参数实时解算出胶囊机器人的横滚角α、俯仰角β、航向角γ，并将解算结果进行实时显示和存储，由此对胶囊机器人的姿态进行实时测定；具体解算公式如下：

式(1)～(6)中：εaa、εba、εca分别表示无线电能发射线圈a工作时线圈a、线圈b、线圈c产生的感应电压的幅值；εab、εbb、εcb分别表示无线电能发射线圈b工作时线圈a、线圈b、线圈c产生的感应电压的幅值；ωa分别表示无线电能发射线圈a激发出的交变磁场的幅值、频率；ωb分别表示无线电能发射线圈b激发出的交变磁场的幅值、频率；na、sa、μa分别表示线圈a的匝数、单匝围成面积、围成区域的有效磁导率；nb、sb、μb分别表示线圈b的匝数、单匝围成面积、围成区域的有效磁导率；nc、sc、μc分别表示线圈c的匝数、单匝围成面积、围成区域的有效磁导率；ωa、ωb、na、sa、μa、nb、sb、μb、nc、sc、μc均为已知量；

步骤四：重复执行步骤一至步骤三，由此持续对胶囊机器人的姿态进行实时测定。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式作出多种变更或修改，但这些变更和修改均落入本发明的保护范围。