[发明专利]一种无线胶囊机器人系统及控制方法

权利要求书

1.一种无线胶囊机器人系统，其特征在于：包括：

胶囊机器人；

永磁环，设于所述胶囊机器人的外侧；

计算机；

驱动装置，与所述计算机连接，并通过所述永磁环驱动胶囊机器人运动；

第一无线通信模块，一端与所述计算机连接，另一端与所述胶囊机器人连接；

无线交变电磁跟踪装置，与所述计算机连接，并与所述胶囊机器人电磁耦合。

2.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述胶囊机器人包括：

壳体，外侧环设所述永磁环；

MCU，设于所述壳体内；

电源模块，与所述MCU连接，并设于所述壳体内；

摄像头，与所述MCU连接，并设于所述壳体内；

温度传感器，与所述MCU连接，并设于所述壳体内；

第二无线通信模块，一端与所述MCU连接，另一端与所述第一无线通信模块连接，并设于所述壳体内；

电磁感应线圈，与所述MCU连接，与所述无线交变电磁跟踪装置电磁耦合，并设于所述壳体内；

PH检测仪，与所述MCU连接，并设于所述壳体的表面。

3.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述驱动装置包括：

机械臂，与所述计算机连接；

永磁体，设于所述机械臂的末端。

4.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述第一无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。

5.如权利要求2所述的一种无线胶囊机器人系统，其特征在于：所述第二无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。

6.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述无线交变电磁跟踪装置包括：

多路交变电磁发射阵列，与所述胶囊机器人电磁耦合；

交变电路，一端与所述多路交变电磁发射阵列连接，另一端与所述计算机连接。

7.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述多路交变电磁发射阵列包括：

若干个交变电磁发射线圈，一端与所述交变电路连接，另一端与所述胶囊机器人电磁耦合。

8.一种无线胶囊机器人系统的控制方法，其特征在于：所述方法需使用如权利要求1至7任一项所述的无线胶囊机器人系统，包括如下步骤：

步骤S10、计算机通过机械臂驱动永磁体运动，永磁体通过磁力驱动永磁环运动，进而联动胶囊机器人运动；

步骤S20、计算机依次通过第一无线通信模块和第二无线通信模块向MCU发送检测指令；

步骤S30、MCU基于接收的所述检测指令控制摄像头拍摄人体内部的照片、控制温度传感器采集人体内部的温度、控制PH检测仪采集人体内部的PH值，并将所述照片、温度以及PH值依次通过第二无线通信模块和第一无线通信模块发送给计算机；

步骤S40、计算机通过无线交变电磁跟踪装置以及电磁感应线圈对胶囊机器人的位置和姿态进行实时跟踪。

9.如权利要求8所述的一种无线胶囊机器人系统的控制方法，其特征在于：所述步骤S40具体包括：

步骤S41、计算机通过交变电路控制各交变电磁发射线圈产生大小为的交变磁场；交变磁场在空间点(x₁,y₁,z₁)的三个坐标方向的分量为：

其中(m,n,p)^T表示交变电磁发射线圈的归一化方向向量，且m²+n²+p²＝1；(a,b,c)^T表示交变电磁发射线圈的中心位置点；B_T表示磁场常量；L表示交变电磁发射线圈的中心位置点到空间点(x₁,y₁,z₁)的距离，且B_x1表示交变磁场在空间点(x₁,y₁,z₁)的x轴方向的分量；B_y1表示交变磁场在空间点(x₁,y₁,z₁)的y轴方向的分量；B_z1表示交变磁场在空间点(x₁,y₁,z₁)的z轴方向的分量；

步骤S42、设电磁感应线圈的中心位置点为(x₂,y₂,z₂)，(v_x,v_y,v_z)^T为电磁感应线圈归一化方向向量，且

当交变电磁发射线圈与电磁感应线圈的归一化方向向量不平行时，计算交变电磁发射线圈产生的磁场强度在电磁感应线圈方向的矢量投影的三个坐标方向的分量：

基于法拉第电磁感应原理，电磁感应线圈的感生电动势为：

其中N表示电磁感应线圈的匝数；表示电磁感应线圈的表面积；表示穿过电磁感应线圈的磁通量；

当交变电磁发射线圈的发射信号为频率为ω的正弦波，则电磁感应线圈感应的电动势为：

其中表示交变电磁发射线圈发射信号的最大幅值；i为正整数；m表示交变电磁发射线圈的总数量，为正整数，且m≥6；ε′_imax表示第i个交变电磁发射线圈的感应电磁力；ε_imax表示第i个交变电磁发射线圈的理论电磁力；

基于电磁感应线圈感应的电动势得到误差方程：

步骤S43、利用粒子群优化算法求解所述误差方程的初值，再利用LM算法对所述初值迭代，得到位姿参数(x,y,z,v_x,v_y,v_z)，基于所述位姿参数对胶囊机器人的位置和姿态进行实时跟踪。