[发明专利]基于三维线圈的胶囊机器人姿态测定系统及方法

权利要求书

1.一种基于三维线圈的胶囊机器人姿态测定方法，其特征在于：该方法是基于一种基于三维线圈的胶囊机器人姿态测定系统实现的，该系统包括置于体外的无线电能发射线圈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模块的电源端、主控电路的电源端、无线信号发射电路的电源端连接；

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

该方法是采用如下步骤实现的：

步骤一：无线电能发射线圈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激发出的交变磁场的幅值、频率；n_a、S_a、μ_a分别表示线圈a的匝数、单匝围成面积、围成区域的有效磁导率；n_b、S_b、μ_b分别表示线圈b的匝数、单匝围成面积、围成区域的有效磁导率；n_c、S_c、μ_c分别表示线圈c的匝数、单匝围成面积、围成区域的有效磁导率；ω_A、ω_B、n_a、S_a、μ_a、n_b、S_b、μ_b、n_c、S_c、μ_c均为已知量；

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