CN115067863B - 基于球型驱动器的无线胶囊内窥镜驱动系统 - Google Patents

Abstract

本申请实施例适用于医疗设备技术领域，提供了一种基于球型驱动器的无线胶囊内窥镜驱动系统，包括无线胶囊内窥镜、球型驱动器和计算机设备；所述无线胶囊内窥镜内部包含有永磁铁；所述球型驱动器包括永磁球体、传感器阵列、至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机；其中：所述计算机设备用于确定所述永磁球体的期望转动增量，根据所述期望转动增量计算各个所述驱动电机待转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述永磁球体产生的磁场驱动所述无线胶囊内窥镜运动。采用上述系统，可以提高无线胶囊内窥镜的驱动控制的灵活性。

Description

基于球型驱动器的无线胶囊内窥镜驱动系统

技术领域

本申请实施例属于医疗设备技术领域，特别是涉及一种基于球型驱动器的无 线胶囊内窥镜驱动系统。

背景技术

无线胶囊内窥镜是一种用于完整消化道检查的重要技术手段，具有无痛苦、 非侵入性等特定。通常，无线胶囊内窥镜只有一颗胶囊大小，它搭载有照明模块、 相机模块、图像处理模块、无线传输模块等。胶囊通过病人吞咽进入病人消化道， 接着在病人体内拍摄图像并实时传输到体外，医生可以根据接收到的图像对消化 道疾病进行诊断。但是，传统的无线胶囊内窥镜在人体内主要依靠肠道的自然蠕 动前进，一次完整的消化道检查需要耗时10-12小时。并且，由于胶囊不受外部控 制，当医生发现疑似病变区域时，不能通过反复观察这个疑似病变区域来确定该 区域是否存在病变，容易造成漏检和误检。

为了实现主动地驱动胶囊在人体肠道内的运动，减少检查时间，有研究人员 开发出了采用体外磁场来控制胶囊在肠道内的运动的检查系统。例如，现有技术 中存在采用两个体外电机来控制永磁球体的旋转，通过控制永磁球体在胶囊所在 处产生的磁场来对胶囊实施控制。但是，上述控制方式灵活性较差。

发明内容

有鉴于此，本申请实施例提供了一种基于球型驱动器的无线胶囊内窥镜驱动 系统，用以提高无线胶囊内窥镜的驱动控制的灵活性。

本申请实施例的第一方面提供了一种基于球型驱动器的无线胶囊内窥镜驱动 系统，包括无线胶囊内窥镜、球型驱动器和计算机设备；所述无线胶囊内窥镜内 部包含有充磁方向与胶囊轴线方向平行的永磁铁；所述球型驱动器包括永磁球体、 安装于所述永磁球体上方的传感器阵列、与所述永磁球体直接接触的至少三个驱 动轮以及分别与一个所述驱动轮刚性连接的驱动电机，任一所述驱动轮与所述永 磁球体的接触点至所述永磁球体的中心点的连线与所述驱动轮的旋转轴垂直安装； 其中：

所述计算机设备用于确定所述永磁球体的期望转动增量，根据所述期望转动 增量计算各个所述驱动电机待转动的角度增量，并控制各个所述驱动电机转动对 应的所述角度增量，以使所述永磁球体产生的磁场驱动所述无线胶囊内窥镜运动。

本申请实施例的第二方面提供了一种计算机设备，包括存储器、处理器以及 存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所 述计算机程序时实现如下方法：

确定永磁球体的期望转动增量，根据所述期望转动增量计算各个驱动电机待 转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述 永磁球体产生的磁场驱动无线胶囊内窥镜运动；

其中，所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁 铁；所述永磁球体与安装于所述永磁球体上方的传感器阵列、与所述永磁球体直 接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机构成一 球型驱动器，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点 的连线与所述驱动轮的旋转轴垂直安装。

本申请实施例的第三方面提供了一种计算机可读存储介质，所述计算机可读 存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如下方法：

确定永磁球体的期望转动增量，根据所述期望转动增量计算各个驱动电机待 转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述 永磁球体产生的磁场驱动无线胶囊内窥镜运动；

其中，所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁 铁；所述永磁球体与安装于所述永磁球体上方的传感器阵列、与所述永磁球体直 接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机构成一 球型驱动器，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点 的连线与所述驱动轮的旋转轴垂直安装。

本申请实施例的第四方面提供了一种计算机程序产品，当所述计算机程序产 品在计算机上运行时，使得所述计算机执行如下方法：

确定永磁球体的期望转动增量，根据所述期望转动增量计算各个驱动电机待 转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述 永磁球体产生的磁场驱动无线胶囊内窥镜运动；

其中，所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁 铁；所述永磁球体与安装于所述永磁球体上方的传感器阵列、与所述永磁球体直 接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机构成一 球型驱动器，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点 的连线与所述驱动轮的旋转轴垂直安装。

与现有技术相比，本申请实施例具有以下优点：

本申请实施例，通过对球型驱动器的运动学进行建模，以及对永磁球体、胶 囊内被动永磁铁之间交互作用的建模，可以利用三个或更多个的驱动电机对永磁 球体实现闭环灵巧控制，进而实现利用永磁球体对胶囊的灵巧控制。相较于现有 技术中往往采用两个驱动电机控制球体的设计，本申请实施例通过采用至少三个 驱动电机来驱动永磁球体这种特别的冗余驱动源设计，提高了球型驱动器对永磁 球体控制的快速性，极大地增强了对永磁球体的旋转运动控制，相应地也就极大 地增强了对无线胶囊内窥镜的灵巧控制。除了在驱动结构设计上的不同之外，本 申请实施例还可以实现闭环控制，通过在永磁球体上方的一个小型传感阵列获取 永磁球体上方的磁场，来对永磁球体的姿态进行计算，从而实现对永磁球体的闭 环灵巧控制。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术 描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是 本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前 提下，还可以根据这些附图获得其他的附图。

图1是本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜驱动系统 的示意图；

图2是本申请实施例提供的一种无线胶囊内窥镜的示意图；

图3是本申请实施例提供的一种球型驱动器的示意图；

图4是本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜驱动方法 的示意图；

图5是本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜驱动方法 中S402的一种实现方式的示意图；

图6是本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜驱动装置 的示意图；

图7是本申请实施例提供的一种计算机设备的示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之 类的具体细节，以便透彻理解本申请实施例。然而，本领域技术人员应当清楚， 在没有这些具体细节的其他实施例中也可以实现本申请。在其他情况中，省略对 众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申 请的描述。

下面通过具体实施例来说明本申请的技术方案。

参照图1，示出了本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜 驱动系统的示意图，该系统可以由检查床101、无线胶囊内窥镜102、机械臂103、 球型驱动器104、大型传感器阵列105和用于进行数据处理的计算机设备(图中未 示出)组成。

其中，无线胶囊内窥镜内部包含图像传感器、无线信号收发模块、微处理模 块和纽扣电池。这些都是传统的无线胶囊内窥镜所具有的模块。除此之外，本申 请实施例中的无线胶囊内窥镜还包含有充磁方向与胶囊轴线方向平行的永磁铁。

如图2所示，是本申请实施例提供的一种无线胶囊内窥镜的示意图，图2中 仅示出了无线胶囊内窥镜102内部的永磁铁1021，该永磁铁可以为圆柱形永磁铁。 需要说明的是，在本申请实施例中，若无特别说明，无线胶囊内窥镜、胶囊内窥 镜以及胶囊均用于表述无线胶囊内窥镜。

在本申请实施例中，如图1所示，机械臂103可以位于检查床101附近，其 包含一末端执行器1031，末端执行器1031可以用于安装球型驱动器104。

如图3所示，是本申请实施例提供的一种球型驱动器的示意图，该球型驱动 器104包括永磁球体1041、安装于永磁球体1041上方的传感器阵列1042、与永 磁球体1041直接接触的至少三个驱动轮1043以及分别与一个驱动轮1043刚性连 接的驱动电机1044。

需要说明的是，图3中示出的驱动轮包括三个，因此分别与一个驱动轮刚性 连接的驱动电机的数量也为三个。三个驱动电机与驱动轮互成120°夹角。在实际 应用中，球型驱动器中包含的驱动电机以及驱动轮的数量也可以多于三个，本申 请实施例对此不作限定。

在本申请实施例中，图3中的三个驱动电机可以通过非铁磁性材料的机械结 构与三个驱动轮刚性连接。三个驱动轮与永磁球体可以直接接触连接。在安装每 一个驱动轮时，应当尽量确保自永磁球体的中心点至对应接触点的连线与该驱动 轮的旋转轴垂直。

永磁球体上方的传感器阵列可以为一小型传感器阵列。该小型传感器阵列与 安装于检查床下方的大型传感器阵列的尺寸结构、功能目的不同。具体地，小型 传感器阵列可以用于测量驱动球体的磁场，感知永磁球体的姿态，其阵列较对较 小，内含的磁传感器数量也较少。例如，小型传感器阵列中可以仅包含有5个三 轴磁传感器。需要说明的是，永磁球体上方的小型传感器阵列并非必须安装在驱 动轮或驱动电机附近，只要是在永磁球体上方或者周围都可以，其原理都是类似 的。图3中示出的安装位置仅是本申请实施例的一种示例，该小型传感器阵列安 装在其他位置也可以实现本方法。例如，小型传感器阵列的安装位置相较于图3 中的位置在高度上有明显差异，或者水平移动偏一些，只要在安装时确定好位姿， 都可以使用。本申请实施例对小型传感器阵列的安装位置不作具体限定。

安装于检查床下方的大型传感器阵列可以用于整个人体中无线胶囊内窥镜所 处环境的磁场测量，定位无线胶囊内窥镜在人体内的位置，其阵列通常较大，内 含的传感器数量也较多。例如，大型传感器阵列中可以包含有80个或更多个的磁 传感器。

在本申请实施例中，用于进行数据处理的计算机设备可以是位于检查床附近 的一台通用计算机。该计算机设备可以由工作人员操作，用于对机械臂和驱动电 机发出控制指令，采集处理小型传感器阵列和大型传感器阵列测量的磁场数据， 显示当前永磁球体的姿态追踪结果和胶囊的定位追踪结果。上述计算机设备中可 以储存有涉及球型驱动器驱动、定位的程序和算法以及其他不可缺少的程序和算 法。

在检查时，病人应平躺于检查床上，口服胶囊，胶囊正常运行，可以拍摄病 人消化道的图像。工作人员可以通过操作计算机设备，驱动永磁球体转动，进而 驱动胶囊在人体消化道内的运动。在此过程中，计算机设备可以确定永磁球体的 期望转动增量，根据期望转动增量计算各个驱动电机待转动的角度增量，并控制 各个驱动电机转动对应的角度增量，以使永磁球体产生的磁场驱动无线胶囊内窥 镜运动。

本申请实施例通过对球型驱动器的运动学进行建模，以及对永磁球体、胶囊 内被动永磁铁之间交互作用的建模，可以利用三个或更多个的驱动电机对永磁球 体实现闭环灵巧控制，进而实现利用永磁球体对胶囊的灵巧控制。

通常来说，对于永磁球体朝向的控制本质上是对永磁球体磁矩向量朝向的控 制，而对于一个单位向量的朝向控制本质上是一个二维运动的控制问题。所以， 现有技术中往往采用两个驱动电机控制球体的设计。而在本申请实施例中，采用 至少三个驱动电机来驱动永磁球体，这是一种冗余驱动源设计。这种特别的冗余 驱动源设计，提高了球型驱动器对永磁球体控制的快速性，极大地增强了对永磁 球体的旋转运动控制，相应地也就极大地增强了对无线胶囊内窥镜的灵巧控制。 除了在驱动结构设计上的不同之外，本申请实施例还可以实现闭环控制，通过在 永磁球体上方的一个小型传感阵列获取永磁球体上方的磁场，来对永磁球体的姿 态进行计算，从而实现对永磁球体的闭环灵巧控制。

下面，对球型驱动器的运动学建模、永磁球体的姿态感知以及永磁球体姿态 朝向的闭环灵巧控制进行详细说明。在下述说明中，统一以球型驱动器中包含三 个驱动电机及对应的三个驱动轮来进行说明。

(1)球型驱动器的运动学建模

为了表示永磁球体的旋转，本申请实施例可以首先定义永磁球体的旋转增量 w_m，它是一个旋转向量，可以表示永磁球体绕向量w _m所在轴转动向量模长‖w _m‖的 角度。

对于给定的永磁球体的期望旋转增量w _m，需要推导出三个驱动电机分别应该 旋转的增量角度θ _a1,θ _a2,θ _a3。

在本申请实施例中，三个驱动电机通过刚性机械结构与各自的驱动轮相连， 这三个驱动轮都可以是全向轮，三个驱动轮与永磁球体直接接触。每个全向轮与 永磁球体之间没有相对滑动。

本申请实施例中的全向轮也可以称作万向轮，是一种经典的结构，由高摩擦 力的轮毂和若干个从动小轮组成。轮毂外圆周处均匀开设多个轮毂齿，每两个轮 毂齿之间装设有一从动小轮，该从动小轮的径向方向与轮毂外圆周的切线方向垂 直。永磁球体能够在与全向轮的接触点实现平行于全向轮转轴方向的驱动旋转(有 摩擦、有力矩、是全向轮驱动球体旋转)。永磁球体也能够在与全向轮的接触点实 现垂直于全向轮转轴方向的自由旋转(永磁球体有些旋转方向是沿着从动小轮的， 可以认为没有摩擦，是全向轮上的从动小轮被动转起来)。

每个全向轮与永磁球体之间没有相对滑动是指全向轮与永磁球体之间是紧紧 接触的，不会打滑。全向轮驱动永磁球体是连续的、有效的，不会出现全向轮转 了，在永磁球体上打滑，球体不转的情形。

设d ₁,d ₂,d ₃表示自永磁球体的中心点出发至各个驱动轮与永磁球体的接触点的朝向单位向量。三个全向轮的轴a ₁,a ₂,a ₃分别与d ₁,d ₂,d ₃垂直。

给定永磁球体的期望旋转增量w _m，在第i个驱动轮接触点的球体表面位移增 量s_i为s _i＝r _mw _m×d _i，其中，r _m为永磁球体的半径。上述位移增量是指接触点在 球体表面移动的距离，是球体表面的弧线距离。

对于第i个驱动轮，球体表面位移增量s _i中平行于驱动轮轴a _i的分量，由全向 驱动轮的从动小轮的旋转实现，而能够引起第i个驱动轮绕其旋转轴a _i旋转的s _i分 量在d _i×a _i方向。

由此可知，对于第i个驱动轮，球体表面的位移增量s _i可以引起第i个驱动轮 绕旋转轴a _i旋转的角度w _ai为：

其中，r _i是第i个驱动轮的半 径，a_i为第i个驱动轮的轴的朝向向量。

按照上述方式，可以依次将三个驱动轮需要绕自身旋转轴旋转的角度都计算 出来，得到w _a1,w _a2,w _a3。

在本申请实施例中，考虑驱动电机与驱动轮之间刚性连接，传动比为c，则可 以计算每个驱动电机待转动的角度增量为θ _ai＝c·w _ai。这样，可以推导出三个驱 动电极待待转动的角度增量θ _a1,θ _a2,θ _a3。

(2)永磁球体的姿态感知

通常，对于永磁球体的姿态朝向的感知本质上是对磁体的磁矩单位向量的朝 向的感知计算。本申请实施例通过对永磁球体的磁场进行建模，可以根据永磁球 体上方的小型传感器阵列的磁场读数计算出磁体磁矩的朝向。

在本申请实施例中，永磁球体的磁矩可以用m _a表示，磁体上方的小型传感器 阵列可以包含有n个三轴磁传感器。对于每一个磁传感器，可以计算它所在的位 置的理论磁场为：

其中，H _i是根据磁偶极子模型整理得到的，H _i与各个三轴磁传感器相对于永 磁球体的中心点的相对位姿相关。当球型驱动器的机械结构确定后，H _i固定不变， 不会再发生变化。

由于有n个磁传感器，可以得到n组b _i＝H _im _a。通过将其排列为矩阵，可以 得到：b＝Hm _a，即：

该矩阵中每三行中的数值为一个三轴磁传感器的读数。

通过计算H的伪逆矩阵H ⁺，可以计算得到永磁球体当前的磁矩朝向： m _a＝H ⁺b。

(3)永磁球体姿态朝向的闭环灵巧控制

通过对球型驱动器的运动学建模和分析，给定永磁球体的期望旋转增量w _m， 可以计算出对应的三个驱动电机待转动的角度增量θ _a1,θ _a2,θ _a3；又通过永磁球体 的磁场建模和分析，给定磁传感器阵列当前的读数b，可以求解出永磁球体的姿态 朝向，也就是永磁球体当前的磁矩朝向m _a。本申请实施例通过将上述二者结合起 来，可以实现对永磁球体姿态朝向的闭环灵活控制。

假设已知永磁球体当前的朝向为m ₀，给定的期望朝向为m _d，则可以定义误差 为：

其中，α为当前朝向m ₀和给定的期望朝向m _d之间的夹角，而

为当前 朝向m ₀转向给定的期望朝向m _d之间的单位旋转向量，由叉乘之后作单位化得到， 即：

这样，e就可以表示当前朝向为m ₀到给定的期望朝向m _d之间的 朝向误差。

在此基础上，本申请实施例引入控制律，可以得出永磁球体的期望转动增量w _m和朝向误差e之间的关系。

在具体实现中，可以采用比例控制、比例微分控制或比例积分控制等任一控 制律确定朝向误差和永磁球体期望转动增量之间的关系式。

示例性地，当控制律为比例控制(Pcontrol)时，w _m＝K _pe。

当控制律为比例微分控制(PDcontrol)时，

这里的e _t表示当前时刻的朝向误差，e _t-1表示上一时刻的朝向误差，τ表示两次迭代 之间的时间间隔。

当控制律为比例积分控制(PIcontrol)时，

其中，K _p、K _d、K _i均为正定矩阵。

当然，上述使用比例控制、比例微分控制或比例积分控制仅为本申请实施例 的一种示例。在实际应用中，还可以采用其他控制律来确定朝向误差和永磁球体 期望转动增量之间的关系式，本申请实施例对此不作限定。

在本申请实施例中，当确定永磁球体的期望转动增量w _m后，可以使用运动学 分析得到的计算方法，计算每个驱动电机应当转动的角度增量θ _a1,θ _a2,θ _a3。

每个驱动电机执行各自的角度增量后停止。

读取此刻永磁球体上方的小型传感器阵列的磁场测量结果b，并根据永磁球体 姿态感知分析中推导的计算方法，计算永磁球体此刻的磁矩朝向m _a，也就是永磁 球体新的当前朝向m ₀。

通过比较新的当前朝向m ₀和给定的期望朝向m _d，可以计算得到新的朝向误差 e，进行下一轮控制迭代。

以此类推，经过多次控制迭代后，直到当前朝向m ₀与给定的期望朝向m _d重合， 或两者之间的误差小于提前预设的误差范围时，可以认为当前朝向收敛到了给定 的期望朝向，此次给定的期望朝向m _d的闭环控制任务完成。

需要说明的是，在一次控制算法的运行过程中，m _d是保持不变的。

参照图4，示出了本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜 驱动方法的示意图，具体可以包括如下步骤：

S401、确定永磁球体的期望转动增量。

需要说明的是，本方法可以应用于如图1所示的基于球型驱动器的无线胶囊 内窥镜驱动系统。关于上述系统的介绍可以参见前述实施例的说明，在此不再赘 述。

图4所示的方法实施例的执行主体可以是上述系统中的计算机设备，该计算 机设备通过执行相应的控制算法，可以实现永磁球体的灵巧控制

在本申请实施例中，永磁球体的期望转动增量w _m是整个控制算法的输入。在 实际应用过程中，用户期望旋转增量是多少便可以设定w _m为多少。

S402、根据所述期望转动增量计算各个驱动电机待转动的角度增量。

在本申请实施例中，计算机设备在确定期望转动增量w _m后，可以计算各个驱 动电机分别需要转动的角度增量是多少(例如，θ _a1,θ _a2,θ _a3)，才能实现永磁球 体转动的增量达到w _m。

在本申请实施例的一种可能的实现方式中，如图5所示，S402中根据所述期 望转动增量计算各个驱动电机待转动的角度增量具体可以包括如下步骤 S4021-S4023：

S4021、针对任一所述驱动轮，根据所述期望转动增量计算所述驱动轮的接触 点在所述永磁球体表面的位移增量。

S4022、根据所述位移增量计算所述驱动轮绕自身的旋转轴旋转的角度。

S4023、根据所述角度和所述驱动电机与所述驱动轮的传动比，计算所述驱动 电机待转动的角度增量。

在本申请实施例中，计算机设备可以采用如下公式计算驱动轮的接触点在永 磁球体表面的位移增量：

s _i＝r _mw _m×d _i

采用如下公式计算驱动轮绕自身的旋转轴旋转的角度：

采用如下公式计算驱动电机待转动的角度增量：

θ _ai＝c·w _ai

其中，s _i为第i个驱动轮的接触点在永磁球体表面的位移增量，r _m为永磁球体 的半径，w _m为期望转动增量，d _i为永磁球体的中心点至第i个驱动轮的接触点的 朝向的单位向量，w _ai为驱动轮绕自身的旋转轴旋转的角度，r _i为第i个驱动轮的半 径，a _i为第i个驱动轮的轴的朝向向量，θ _ai为驱动电机待转动的角度增量，c为驱 动电机与驱动轮的传动比。

S403、控制各个所述驱动电机转动对应的所述角度增量，以使所述永磁球体 产生的磁场驱动无线胶囊内窥镜运动。

在确定各个驱动电机应当转动角度增量后，计算机设备可以控制每个驱动电 机转动对应的角度增量，以使永磁球体在转动过程中产生的磁场可以驱动无线胶 囊内窥镜运动。

在本申请实施例中，计算机设备控制每个驱动电机转动对应的角度增量后， 可能并不能一次性地使得永磁球体转动至给定的期望朝向，需要进行多轮控制迭 代。

因此，在各个驱动电机转动对应的角度增量并停止后，计算机设备可以计算 永磁球体当前的磁矩朝向m ₀；根据永磁球体当前的磁矩朝向m ₀和给定的期望朝向 m _d计算朝向误差e，并基于朝向误差e确定永磁球体新的期望转动增量并根据新的 期望转动增量控制各个驱动电机转动。

在具体实现中，在各个驱动电机转动对应的角度增量并停止后，计算机设备 可以读取各个磁传感器的读数，根据读数计算传感器阵列的磁场测量值，从而根 据磁场测量值计算永磁球体当前的磁矩朝向。

在本申请实施例中，小型传感器阵列中的多个磁传感器可以均为三轴磁传感 器，计算机设备可以将各个三轴磁传感器的读数排列为矩阵b。这样，矩阵b中每 三行中的数值表示一个三轴磁传感器的读数。

通过确定各个三轴磁传感器的相对位姿关联值H _i，计算机设备可以根据矩阵b和H _i计算永磁球体当前的磁矩朝向。上述相对位姿关联值H _i是根据磁偶极子模型 整理得到的，与各个三轴磁传感器相对于永磁球体的中心点的相对位姿相关，当 球型驱动器的机械结构确定后，H _i固定不变。

在具体实现中，计算机设备可以采用如下公式计算永磁球体当前的磁矩朝向：

m _a＝H ⁺b

其中，m _a为永磁球体当前的磁矩朝向，H ⁺为基于多个三轴磁传感器的相对位 姿关联值计算得到的伪逆矩阵。

需要说明的是，计算机设备可以采用预设控制律，确定朝向误差和永磁球体 新的期望转动增量之间的关系式；其中，上述预设控制律可以包括比例控制、比 例微分控制或比例积分控制等。

需要说明的是，上述实施例中各步骤的序号的大小并不意味着执行顺序的先 后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实 施过程构成任何限定。

参照图6，示出了本申请实施例提供的一种基于球型驱动器的无线胶囊内窥镜 驱动装置的示意图，该装置可以应用于前述各个实施例介绍的基于球型驱动器的 无线胶囊内窥镜驱动系统中，该系统包括无线胶囊内窥镜、球型驱动器和计算机 设备；所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁铁； 所述球型驱动器包括永磁球体、安装于所述永磁球体上方的传感器阵列、与所述 永磁球体直接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动 电机，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点的连线 与所述驱动轮的旋转轴垂直安装。该装置具体可以包括确定模块601、计算模块 602和控制模块603，其中：

确定模块601，用于确定所述永磁球体的期望转动增量；

计算模块602，用于根据所述期望转动增量计算各个所述驱动电机待转动的角 度增量；

控制模块603，用于控制各个所述驱动电机转动对应的所述角度增量，以使所 述永磁球体产生的磁场驱动所述无线胶囊内窥镜运动。

在本申请实施例中，所述计算模块602具体可以用于：针对任一所述驱动轮， 根据所述期望转动增量计算所述驱动轮的接触点在所述永磁球体表面的位移增量； 根据所述位移增量计算所述驱动轮绕自身的旋转轴旋转的角度；根据所述角度和 所述驱动电机与所述驱动轮的传动比，计算所述驱动电机待转动的角度增量。

在本申请实施例中，所述计算模块602具体可以采用如下公式计算所述驱动 轮的接触点在所述永磁球体表面的位移增量：

S _i＝r _mw _m×d _i

所述计算模块602具体可以采用如下公式计算所述驱动轮绕自身的旋转轴旋 转的角度：

所述计算模块602具体可以采用如下公式计算所述驱动电机待转动的角度增 量：

θ _ai＝c·w _ai

其中，s _i为第i个驱动轮的接触点在所述永磁球体表面的位移增量，r _m为所述 永磁球体的半径，w _m为所述期望转动增量，d _i为所述永磁球体的中心点至第i个 驱动轮的接触点的朝向的单位向量，w _ai为所述驱动轮绕自身的旋转轴旋转的角度， r _i为第i个驱动轮的半径，a _i为第i个驱动轮的轴的朝向向量，θ _ai为所述驱动电机 待转动的角度增量，c为所述驱动电机与所述驱动轮的传动比。

在本申请实施例中，所述控制模块603还可以用于：在各个所述驱动电机转 动对应的所述角度增量并停止后，计算所述永磁球体当前的磁矩朝向；根据所述 永磁球体当前的磁矩朝向和给定的期望朝向计算朝向误差；基于所述朝向误差确 定所述永磁球体新的期望转动增量并根据新的所述期望转动增量控制各个所述驱 动电机转动。

在本申请实施例中，所述传感器阵列包括多个磁传感器，所述计算模块602 还可以用于：在各个所述驱动电机转动对应的所述角度增量并停止后，读取各个 所述磁传感器的读数；根据所述读数计算所述传感器阵列的磁场测量值；根据所 述磁场测量值计算所述永磁球体当前的磁矩朝向。

在本申请实施例中，多个所述磁传感器可以均为三轴磁传感器，所述计算模 块602还可以用于：将各个所述三轴磁传感器的读数排列为矩阵，所述矩阵中每 三行中的数值为一个所述三轴磁传感器的读数；确定各个所述三轴磁传感器的相 对位姿关联值，所述相对位姿关联值是根据磁偶极子模型整理得到的，与各个所 述三轴磁传感器相对于所述永磁球体的中心点的相对位姿相关，当所述球型驱动 器的机械结构确定后，所述相对位姿关联值固定不变；根据所述矩阵和多个所述 三轴磁传感器的所述相对位姿关联值计算所述永磁球体当前的磁矩朝向。

在本申请实施例中，所述计算模块602具体可以采用如下公式计算所述永磁 球体当前的磁矩朝向：

m _a＝H ⁺b

其中，m _a为所述永磁球体当前的磁矩朝向，H ⁺为基于多个所述三轴磁传感器 的所述相对位姿关联值计算得到的伪逆矩阵，b为所述矩阵。

在本申请实施例中，所述计算模块602还可以用于：采用预设控制律，确定 所述朝向误差和所述永磁球体新的所述期望转动增量之间的关系式；其中，所述 预设控制律包括比例控制、比例微分控制或比例积分控制。

对于装置实施例而言，由于其与方法实施例基本相似，所以描述得比较简单， 相关之处参见方法实施例部分的说明即可。

参照图7，示出了本申请实施例提供的一种计算机设备的示意图。该计算机设 备700可以应用于前述各个实施例介绍的基于球型驱动器的无线胶囊内窥镜驱动 系统中，关于该系统的介绍可以参见前述各个实施例的说明，在此不再赘述。

如图7所示，本申请实施例中的计算机设备700包括：处理器710、存储器 720以及存储在所述存储器720中并可在所述处理器710上运行的计算机程序721。 所述处理器710执行所述计算机程序721时实现上述基于球型驱动器的无线胶囊 内窥镜驱动方法各个实施例中的步骤，例如图4所示的步骤S401至S403。或者， 所述处理器710执行所述计算机程序721时实现上述各装置实施例中各模块/单元 的功能，例如图6所示模块601至603的功能。

示例性的，所述计算机程序721可以被分割成一个或多个模块/单元，所述一 个或者多个模块/单元被存储在所述存储器720中，并由所述处理器710执行，以 完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机 程序指令段，该指令段可以用于描述所述计算机程序721在所述计算机设备700 中的执行过程。例如，所述计算机程序721可以被分割成确定模块、计算模块和 控制模块，各模块具体功能如下：

确定模块，用于确定所述永磁球体的期望转动增量；

计算模块，用于根据所述期望转动增量计算各个所述驱动电机待转动的角度 增量；

控制模块，用于控制各个所述驱动电机转动对应的所述角度增量，以使所述 永磁球体产生的磁场驱动所述无线胶囊内窥镜运动。

所述计算机设备700可以是桌上型计算机、云端服务器等计算设备。所述计 算机设备700可包括，但不仅限于，处理器710、存储器720。本领域技术人员可 以理解，图7仅仅是计算机设备700的一种示例，并不构成对计算机设备700的 限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件， 例如所述计算机设备700还可以包括输入输出设备、网络接入设备、总线等。

所述处理器710可以是中央处理单元(Central Processing Unit，CPU)，还可 以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集 成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者 晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也 可以是任何常规的处理器等。

所述存储器720可以是所述计算机设备700的内部存储单元，例如计算机设 备700的硬盘或内存。所述存储器720也可以是所述计算机设备700的外部存储 设备，例如所述计算机设备700上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等等。 进一步地，所述存储器720还可以既包括所述计算机设备700的内部存储单元也 包括外部存储设备。所述存储器720用于存储所述计算机程序721以及所述计算 机设备700所需的其他程序和数据。所述存储器720还可以用于暂时地存储已经 输出或者将要输出的数据。

本申请实施例还公开了一种计算机设备，包括存储器、处理器以及存储在所 述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机 程序时实现如前述各个实施例所述的基于球型驱动器的无线胶囊内窥镜驱动方法。

本申请实施例还公开了一种计算机可读存储介质，所述计算机可读存储介质 存储有计算机程序，所述计算机程序被处理器执行时实现如前述各个实施例所述 的基于球型驱动器的无线胶囊内窥镜驱动方法。

本申请实施例还公开了一种计算机程序产品，当所述计算机程序产品在计算 机上运行时，使得所述计算机执行前述各个实施例所述的基于球型驱动器的无线 胶囊内窥镜驱动方法。

以上所述实施例仅用以说明本申请的技术方案，而非对其限制。尽管参照前 述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然 可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行 等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施 例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (8)

1.一种基于球型驱动器的无线胶囊内窥镜驱动系统，其特征在于，包括无线胶囊内窥镜、球型驱动器和计算机设备；所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁铁；所述球型驱动器包括永磁球体、安装于所述永磁球体上方的传感器阵列、与所述永磁球体直接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点的连线与所述驱动轮的旋转轴垂直安装；其中：

所述计算机设备用于确定所述永磁球体的期望转动增量，根据所述期望转动增量计算各个所述驱动电机待转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述永磁球体产生的磁场驱动所述无线胶囊内窥镜运动；

其中，所述根据所述期望转动增量计算各个所述驱动电机待转动的角度增量，包括：

针对任一所述驱动轮，根据所述期望转动增量计算所述驱动轮的接触点在所述永磁球体表面的位移增量；

根据所述位移增量计算所述驱动轮绕自身的旋转轴旋转的角度；

根据所述角度和所述驱动电机与所述驱动轮的传动比，计算所述驱动电机待转动的角度增量；

所述计算机设备采用如下公式计算所述驱动轮的接触点在所述永磁球体表面的位移增量：

s _i＝ _mw _m×d _i

所述计算机设备采用如下公式计算所述驱动轮绕自身的旋转轴旋转的角度：

所述计算机设备采用如下公式计算所述驱动电机待转动的角度增量：

θ _ai＝c·w _ai

其中，s _i为第i个驱动轮的接触点在所述永磁球体表面的位移增量，r _m为所述永磁球体的半径，w _m为所述期望转动增量，d _i为所述永磁球体的中心点至第i个驱动轮的接触点的朝向的单位向量，w _ai为所述驱动轮绕自身的旋转轴旋转的角度，r _i为第i个驱动轮的半径，a _i为第i个驱动轮的轴的朝向向量，θ _ai为所述驱动电机待转动的角度增量，c为所述驱动电机与所述驱动轮的传动比。

2.根据权利要求1所述的系统，其特征在于，所述计算机设备还用于：

在各个所述驱动电机转动对应的所述角度增量并停止后，计算所述永磁球体当前的磁矩朝向；

根据所述永磁球体当前的磁矩朝向和给定的期望朝向计算朝向误差；

基于所述朝向误差确定所述永磁球体新的期望转动增量并根据新的所述期望转动增量控制各个所述驱动电机转动。

3.根据权利要求2所述的系统，其特征在于，所述传感器阵列包括多个磁传感器，所述计算机设备具体用于：

在各个所述驱动电机转动对应的所述角度增量并停止后，读取各个所述磁传感器的读数；

根据所述读数计算所述传感器阵列的磁场测量值；

根据所述磁场测量值计算所述永磁球体当前的磁矩朝向。

4.根据权利要求3所述的系统，其特征在于，多个所述磁传感器均为三轴磁传感器，所述计算机设备具体用于：

将各个所述三轴磁传感器的读数排列为矩阵，所述矩阵中每三行中的数值为一个所述三轴磁传感器的读数；

确定各个所述三轴磁传感器的相对位姿关联值，所述相对位姿关联值是根据磁偶极子模型整理得到的，与各个所述三轴磁传感器相对于所述永磁球体的中心点的相对位姿相关，当所述球型驱动器的机械结构确定后，所述相对位姿关联值固定不变；

根据所述矩阵和多个所述三轴磁传感器的所述相对位姿关联值计算所述永磁球体当前的磁矩朝向。

5.根据权利要求4所述的系统，其特征在于，所述计算机设备采用如下公式计算所述永磁球体当前的磁矩朝向：

m _a＝ ⁺b

其中，m _a为所述永磁球体当前的磁矩朝向，H ⁺为基于多个所述三轴磁传感器的所述相对位姿关联值计算得到的伪逆矩阵，b为所述矩阵。

6.根据权利要求2所述的系统，其特征在于，所述计算机设备具体用于：

采用预设控制律，确定所述朝向误差和所述永磁球体新的所述期望转动增量之间的关系式；其中，所述预设控制律包括比例控制、比例微分控制或比例积分控制。

7.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如下方法：

确定永磁球体的期望转动增量，根据所述期望转动增量计算各个驱动电机待转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述永磁球体产生的磁场驱动无线胶囊内窥镜运动；

其中，所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁铁；所述永磁球体与安装于所述永磁球体上方的传感器阵列、与所述永磁球体直接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机构成一球型驱动器，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点的连线与所述驱动轮的旋转轴垂直安装；

其中，所述根据所述期望转动增量计算各个所述驱动电机待转动的角度增量，包括：

针对任一所述驱动轮，根据所述期望转动增量计算所述驱动轮的接触点在所述永磁球体表面的位移增量；

根据所述位移增量计算所述驱动轮绕自身的旋转轴旋转的角度；

根据所述角度和所述驱动电机与所述驱动轮的传动比，计算所述驱动电机待转动的角度增量；

所述计算机设备采用如下公式计算所述驱动轮的接触点在所述永磁球体表面的位移增量：

s _i＝ _mw _m×d _i

所述计算机设备采用如下公式计算所述驱动轮绕自身的旋转轴旋转的角度：

所述计算机设备采用如下公式计算所述驱动电机待转动的角度增量：

θ _ai＝c·w _ai

其中，s _i为第i个驱动轮的接触点在所述永磁球体表面的位移增量，r _m为所述永磁球体的半径，w _m为所述期望转动增量，d _i为所述永磁球体的中心点至第i个驱动轮的接触点的朝向的单位向量，w _ai为所述驱动轮绕自身的旋转轴旋转的角度，r _i为第i个驱动轮的半径，a _i为第i个驱动轮的轴的朝向向量，θ _ai为所述驱动电机待转动的角度增量，c为所述驱动电机与所述驱动轮的传动比。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如下方法：

确定永磁球体的期望转动增量，根据所述期望转动增量计算各个驱动电机待转动的角度增量，并控制各个所述驱动电机转动对应的所述角度增量，以使所述永磁球体产生的磁场驱动无线胶囊内窥镜运动；

其中，所述无线胶囊内窥镜内部包含有充磁方向与胶囊轴线方向平行的永磁铁；所述永磁球体与安装于所述永磁球体上方的传感器阵列、与所述永磁球体直接接触的至少三个驱动轮以及分别与一个所述驱动轮刚性连接的驱动电机构成一球型驱动器，任一所述驱动轮与所述永磁球体的接触点至所述永磁球体的中心点的连线与所述驱动轮的旋转轴垂直安装；

其中，所述根据所述期望转动增量计算各个所述驱动电机待转动的角度增量，包括：

针对任一所述驱动轮，根据所述期望转动增量计算所述驱动轮的接触点在所述永磁球体表面的位移增量；

根据所述位移增量计算所述驱动轮绕自身的旋转轴旋转的角度；

根据所述角度和所述驱动电机与所述驱动轮的传动比，计算所述驱动电机待转动的角度增量；

计算机设备采用如下公式计算所述驱动轮的接触点在所述永磁球体表面的位移增量：

s _i＝ _mw _m×d _i

所述计算机设备采用如下公式计算所述驱动轮绕自身的旋转轴旋转的角度：

所述计算机设备采用如下公式计算所述驱动电机待转动的角度增量：

θ _ai＝c·w _ai

其中，s _i为第i个驱动轮的接触点在所述永磁球体表面的位移增量，r _m为所述永磁球体的半径，w _m为所述期望转动增量，d _i为所述永磁球体的中心点至第i个驱动轮的接触点的朝向的单位向量，w _ai为所述驱动轮绕自身的旋转轴旋转的角度，r _i为第i个驱动轮的半径，a _i为第i个驱动轮的轴的朝向向量，θ _ai为所述驱动电机待转动的角度增量，c为所述驱动电机与所述驱动轮的传动比。

Images