[發明專利]一種無線膠囊機器人系統及控制方法

在審

• 申請號: 202010722229.1
• 申請日: 2020-07-24
• 公開（公告）號: CN111839431A
• 公開（公告）日: 2020-10-30
• 發明（設計）人: 陽萬安;戴厚德
• 申請（專利權）人: 福建世新機器人科技有限公司
• 主分類號: A61B1/04
• 分類號: A61B1/04;A61B1/045
• 代理公司: 泉州市文華專利代理有限公司 35205
• 代理人: 陳雪瑩

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)，基于所述位姿參數對膠囊機器人的位置和姿態進行實時跟蹤。