技术领域
本申请涉及医疗器械领域，特别涉及一种胶囊内镜运动控制方法及系统。
背景技术
胶囊内镜是一种做成胶囊形状与大小的内窥镜，用于检查人体消化道的健康状况。胶囊内镜内置了摄像装置和信号传输装置，当其被受检者吞服后，可以沿消化道运动，对消化道内壁进行拍摄检查，并将拍摄的图像传输至外部终端，以便于医生通过胶囊内镜拍摄的图像，对受检者的消化道的病情做出诊断。
但是，目前市售的产品并不能实现由医生主动控制胶囊内镜在人体消化道中的运动状态，被吞服后的胶囊内镜只能依靠人体消化道的自然蠕动而进行随机的无规则运动。由此，一方面医生无法获取指定位置的清晰图片，因而会增加漏诊、误诊的风险；更重要的另一方面，该类产品因不能主动控制而无法适用于人体胃部高动态环境的检测。因此，如何主动控制胶囊内镜在受检者体内的运动状态，使其可以应用于人体胃部病情的诊断是十分重要的。
针对该问题，现有技术中提出了一种解决方案。该方案通过根据胶囊内镜的反馈信息，不断调节位于人体外部的永磁体和电磁铁所产生的磁场，由于胶囊内镜内置了永磁体，其受到磁场的电磁作用力，因此可以通过调节磁场来调节胶囊内镜在磁场中的运动状态。然而，该方案中由于永磁体所产生的磁场是固定不变的，而电磁铁对磁场的调节能力非常有限，因此该方法对胶囊内镜的控制能力也是十分有限的；并且该方案只在单一方向——竖直方向上对胶囊内镜进行了受力分析与控制，因此其只适用于理论静态环境中竖直方向上的运动状态调整，而不适用于人体胃部不断蠕动和收缩的实际高动态环境。
发明内容
本申请的目的在于提供一种胶囊内镜运动控制方法及系统，以便有效地主动控制胶囊内镜的运动状态，从而使其适用于人体胃部的病情检查。
为解决上述技术问题，本申请提供一种胶囊内镜运动控制方法，包括：
接收胶囊内镜内置的传感器所检测到的所述胶囊内镜的实际运动状态；所述胶囊内镜位于电磁线圈所产生的磁场中；
根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压；
根据所述控制电压调节所述电磁线圈的供电电压，以便改变所述胶囊内镜的受力状况，直至所述胶囊内镜达到所述期望运动状态。
其中，所述接收胶囊内镜内置的传感器所检测到的所述胶囊内镜的实际运动状态包括：
接收所述胶囊内镜内置的三轴加速度计检测到的所述胶囊内镜的实际加速度；
接收所述胶囊内镜内置的三轴角速度计检测到的所述胶囊内镜的实际角速度。
其中，所述根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压包括：
根据所述实际加速度和输入的期望位置，利用位置控制模型计算梯度线圈的控制电压；
根据所述实际角速度和输入的期望姿态角，利用姿态控制模型计算匀场线圈的控制电压。
其中，所述根据所述实际加速度和输入的期望位置，利用位置控制模型计算梯度线圈的控制电压的步骤包括：
将所述实际加速度经二次积分计算得到所述胶囊内镜当前时刻的实际位置；
根据所述实际位置进行受力分析计算得到所述梯度线圈的平衡电流；
根据所述实际位置和所述期望位置计算得到位置误差；
将所述位置误差代入所述位置控制模型中计算得到所述梯度线圈的调节电流；
根据所述平衡电流和所述调节电流计算得到所述梯度线圈的控制电流；
根据所述梯度线圈的控制电流和驱动所述梯度线圈的电阻阻值，计算得到当前时刻梯度线圈的控制电压。
其中，所述根据所述实际角速度和输入的期望姿态角，利用姿态控制模型计算所述匀场线圈的控制电压的步骤包括：
根据所述实际加速度计算得到所述胶囊内镜的初始姿态角；
根据所述初始姿态角和所述实际角速度计算得到所述胶囊内镜当前时刻的实际姿态角；
根据所述实际姿态角和所述期望姿态角计算得到姿态角误差；
将所述姿态角误差代入所述姿态控制模型中计算得到所述匀场线圈的控制电流；
根据所述匀场线圈的控制电流和驱动所述匀场线圈的电阻阻值，计算得到当前时刻所述匀场线圈的控制电压。
其中，所述根据所述初始姿态角和所述实际角速度计算得到所述胶囊内镜当前时刻的实际姿态角包括：
根据所述初始姿态角确定所述胶囊内镜初始时刻姿态的四元数；
根据所述初始时刻姿态的四元数和所述实际角速度，计算得到所述胶囊内镜当前时刻姿态的四元数；
根据所述当前时刻姿态的四元数计算得到所述胶囊内镜当前时刻的实际姿态角。
其中，所述根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压包括：
根据所述实际运动状态和输入的期望运动状态，在所述控制模型中采用积分分离且带死区的PID控制算法计算所述电磁线圈的控制电压。
本申请还提供了一种胶囊内镜运动控制系统，包括：
电磁线圈：用于产生磁场；
胶囊内镜：用于在所述磁场的作用下在受检者消化道内运动；所述胶囊内镜内置有传感器；
所述传感器：用于检测所述胶囊内镜的实际运动状态；
驱动电路：用于向所述电磁线圈提供供电电压；
CPU：用于接收所述胶囊内镜的实际运动状态，根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压；根据所述控制电压调节所述驱动电路为所述电磁线圈提供的供电电压，以便改变所述胶囊内镜的受力状况，直至所述胶囊内镜达到所述期望运动状态。
其中，所述电磁线圈包括：
梯度线圈：用于产生均匀场强梯度磁场；
匀场线圈：用于产生均匀磁场强度磁场；
所述磁场包括所述均匀场强梯度磁场和所述均匀磁场强度磁场叠加的组合磁场。
其中，所述传感器包括三轴加速度计和三轴角速度计；
所述CPU在接收所述胶囊内镜的实际运动状态时具体用于：
接收所述三轴加速度计检测到的所述胶囊内镜的实际加速度；接收所述三轴角速度计检测到的所述胶囊内镜的实际角速度。
本申请所提供的胶囊内镜运动控制方法中，通过接收胶囊内镜内置的传感器所检测到的所述胶囊内镜的实际运动状态；所述胶囊内镜位于电磁线圈所产生的磁场中；根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压；根据所述控制电压调节所述电磁线圈的供电电压，以便改变所述胶囊内镜的受力状况，直至所述胶囊内镜达到所述期望运动状态。
可见，相比于现有技术，本申请所提供的胶囊内镜运动控制方法中利用电磁线圈产生磁场，可以通过调节电磁线圈的供电电压有效地对磁场进行调整，从而可以有效避免现有技术中采用永磁体加电磁铁的控制方式中磁场调节能力非常有限的弊端。因此，本申请可以在胶囊内镜被吞服后，通过传感器检测到的实时运动状态建立闭环控制，根据运动状态误差在控制模型中的计算结果，不断调整电磁线圈的供电电压进而调整磁场，从而改变胶囊内镜在磁场中的受力情况以改变其运动状态，直至胶囊内镜达到指定的期望运动状态。由此可见，本申请所提供的胶囊内镜运动控制方法可以有效实现对胶囊内镜的主动控制，从而使其可以适用于人体胃部的高动态运动环境。本申请所提供的胶囊内镜运动控制系统可以实现上述胶囊内镜运动控制方法，同样具有上述有益效果。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对本申请实施例描述中需要使用的附图作简要的介绍。当然，下面有关本申请实施例的附图描述的仅仅是本申请中的一部分实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图，所获得的其他附图也属于本申请的保护范围。
图1为本申请实施例所提供的一种胶囊内镜运动控制方法的流程图；
图2为本申请实施例所提供的一种电磁线圈安装结构的主视图；
图3为采用图2所示电磁线圈安装方式时胶囊内镜运动控制方法的流程图；
图4为根据实际加速度和输入的期望位置，利用位置控制模型计算梯度线圈的控制电压的流程图；
图5为根据实际角速度和输入的期望姿态角，利用姿态控制模型计算匀场电磁线圈的控制电压的流程图；
图6为根据初始姿态角和实际角速度计算得到胶囊内镜当前时刻的实际姿态角的流程图；
图7为积分分离且带死区的PID控制算法的流程图；
图8为本申请所提供的一种胶囊内镜运动控制系统的结构框图。
具体实施方式
为了对本申请实施例中的技术方案进行更加清楚、完整地描述，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行介绍。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
请参考图1，图1为本申请实施例所提供的一种胶囊内镜运动控制方法的流程图，主要包括以下步骤：
步骤101：接收胶囊内镜内置的传感器所检测到的胶囊内镜的实际运动状态；所述胶囊内镜位于电磁线圈所产生的磁场中。
本申请所提供的方法中，通过在人体外部设置电磁线圈来产生磁场，对内置有永磁体的胶囊内镜产生电磁作用力，以便调节胶囊内镜的运动状态。电磁线圈可以通过调节供电电压来调节其电流，进而可以有效地调整磁场，因此，采用电磁线圈的磁场调节控制方式可以对胶囊内镜实现有效的控制。
这里所说的电磁线圈，可以搭配使用梯度线圈和匀场线圈。梯度线圈可以产生均匀场强梯度磁场，匀场线圈可以产生均匀磁场强度磁场，两者共同使用时，可以形成均匀场强梯度磁场和均匀磁场强度磁场的组合磁场，相比于单一形式的磁场可以更加方便地实现对磁场的调节。当然，电磁线圈采用不同的个数或者不同的安装位置，所形成的磁场是不同的，本领域技术人员可以根据实际的使用情况自行选择，本申请实施例对此并不做限定，只要可以通过调节电磁线圈的电流来有效调节磁场，进而可以调节胶囊内镜在磁场中运动状态即可。
如前所述，胶囊内镜内置有传感器可以检测胶囊内镜在受检者体内的运动状态。在位置控制过程中常用的运动状态物理量有加速度、速度和位置，由于速度和位置可以由加速度计算得到，而对于胶囊内镜来说，其体积越小越好，因此优选地，可以利用三轴加速度计对胶囊内镜的加速度进行检测，并根据加速度计算得到位置，从而以便于在后续步骤中对其位置进行控制。
另一方面，当医生在控制胶囊内镜运动时，为了能更加全面和清楚地观察指定点的图像，除了要将胶囊内镜调节到其指定的位置外，可能还需要对胶囊内镜的姿态进行调整，例如将胶囊内镜带有摄像装置的一端向左上方倾斜一定角度等等，即对胶囊内镜的运动控制还可以包括对胶囊内镜运动姿态的控制。具体地，可以通过对俯仰角、横滚角、偏航角这三个姿态角的控制来实现对胶囊内镜姿态的控制。而由于物体的姿态角无法直接由传感器检测，因此可以通过三轴角速度计(一般指惯性陀螺仪)测出角速度，进而通过姿态解算得到姿态角，以便在后续步骤中对胶囊内镜的姿态进行控制。
步骤102：根据实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压。
本申请所提供的方法中采用闭环反馈控制，即通过将检测到的实际运动状态和输入的期望运动状态进行做差比较，只要两者之间存在误差，就根据误差不断调节磁场，改变胶囊内镜的受力情况，直至其运动状态达到期望的运动状态。如前所述，对胶囊内镜的运动可以包括其位置控制和姿态控制，因此，可以分别建立位置控制模型和姿态控制模型，根据位置控制模型可以由位置误差计算出相应电磁线圈的控制电压，根据姿态控制模型可以由姿态误差计算出相应电磁线圈的控制电压。
不同类型和不同安装位置的电磁线圈，对磁场的影响不同，因此采用不同数目、类型和安装位置的电磁线圈所产生的的磁场可能是不同的。对于按照某种具体设置方式而产生的磁场，可以通过对理论建模而获得，而具体哪些电磁线圈可以方便地对胶囊内镜的位置进行控制，而哪些电磁线圈又可以方便地对胶囊内镜的姿态进行控制，可以由本领域技术人员根据电磁线圈的安装方案具体分析计算和设置，本申请对此并不做限定。
当依据位置控制模型或者姿态控制模型进行计算和控制时，具体可以采用控制工程中常用的PID控制算法，也可以采用其他控制算法如模糊控制、神经网络控制等，本申请对具体的控制算法并不做限定。
步骤103：根据控制电压调节电磁线圈的供电电压，以便改变胶囊内镜的受力状况，直至胶囊内镜达到期望运动状态。
这里要说明的是，在电路中一般是通过调节电压来实现电流的调节。当根据控制模型计算得到各个电磁线圈的控制电压后，便可以将各个电磁线圈的供电电压分别调整为控制电压，以改变电磁线圈的通电电流，进而改变磁场。
由于驱动电磁线圈的电阻是固定的，因此改变电磁线圈的供电电压后，电磁线圈的电流及其产生的磁场也发生了相应变化，处于磁场之中的胶囊内镜的受力情况和运动状态也因此改变。具体地，可以采用PWM方式对各个电磁线圈的供电电压进行调整。
通过不断地重复以上步骤，即不断地计算运动状态误差进而得到控制电压，可以不断调整胶囊内镜的运动状态，直到其达到输入的期望运动状态。
可见，本申请实施例所提供的胶囊内镜运动控制方法，利用电磁线圈产生用于改变胶囊内镜受力情况的磁场，因而可以通过调整电磁线圈的电压实现对磁场的有效调整，进而实现对胶囊内镜的有效的主动控制。本申请实施例具体是根据将胶囊内镜的运动状态误差代入控制模型后的计算结果，对电磁线圈的电压进行相应调整的，从而可以有效地实现对胶囊内镜运动状态的主动控制，使其可以适用于人体胃部的高动态运动环境。
请参考图2，图2为本申请实施例所提供的一种电磁线圈安装结构的主视图。
如图2所示，本申请实施例所提供的一种电磁线圈安装结构中，采用了四个电磁线圈，并均匀分布在竖直平面内的圆形支架上。其中，位于竖直直线上的上下两个电磁线圈为梯度线圈，位于水平直线上的左右两个电磁线圈为匀场线圈。当图2所示的电磁线圈磁场在使用时，受检者沿垂直纸面方向横躺于图2所示的圆形支架中间，四个电磁线圈分别位于受检者的上下左右四侧。
采用如图2所示的安装方式设置电磁线圈，具有简单易操作的优点。其中，两个梯度线圈可以用于调整磁场梯度，便于对胶囊内镜的位置进行调节；而另两个匀场线圈可以用于调整磁场强度，便于对胶囊内镜的姿态进行调整。
当然，如前所述，本领域技术人员也可以采用别的方式设置电磁线圈，并可以根据理论分析和建模等方法得到相应的磁场模型，进而得到相应的一套控制模型和算法。
请参考图3，图3为采用图2所示电磁线圈安装方式时胶囊内镜运动控制方法的流程图，主要包括以下步骤：
步骤301：接收胶囊内镜内置的三轴加速度计检测到的胶囊内镜的实际加速度；接收胶囊内镜内置的三轴角速度计检测到的胶囊内镜的实际角速度。
本申请实施例在采用图2所示的电磁线圈的安装结构的同时，在胶囊内镜中内置了三轴加速度计和三轴角速度计，分别用于检测胶囊内镜的加速度和角速度。通过胶囊内镜的信号传输装置，可以接收到三轴加速度计检测到的加速度和三轴角速度计检测到的角速度，以便于后续步骤中对胶囊内镜位置和姿态的控制。
步骤302：根据实际加速度和输入的期望位置，利用位置控制模型计算梯度线圈的控制电压；根据实际角速度和输入的期望姿态角，利用姿态控制模型计算匀场线圈的控制电压。
本申请实施例中上下两个电磁线圈为梯度线圈，可以产生均匀场强梯度磁场，通过调节梯度线圈的供电电压，可以对组合磁场进行梯度调整，以便对胶囊内镜产生合适的驱动力以驱动其向期望位置运动。根据检测到的实际加速度，即可以计算出实际位置进而得到位置误差，当存在位置误差时，就对梯度线圈的供电电压进行调整，并且具体梯度线圈供电电压的调整量是根据位置控制模型计算来的。相关的计算过程将在图4中进行介绍。
另一方面，本申请实施例中左右两个电磁线圈为匀场线圈，可以产生均匀磁场强度磁场，通过调节匀场线圈的供电电压，可以对组合磁场进行场强调整，以便对胶囊内镜产生合适的磁转矩以使其调整到期望姿态。根据检测到的实际角速度，便可以计算得到实际姿态角，进而得到姿态角误差，当存在姿态角误差时，对匀场线圈的供电电压进行调整，具体匀场线圈供电电压的调整量是根据姿态控制模型计算来的。相关的计算过程将在图5中进行介绍。
步骤303：根据梯度线圈的控制电压调节梯度线圈的供电电压，根据匀场线圈的控制电压调节匀场线圈的供电电压，直至胶囊内镜达到期望运动状态。
本步骤的相关内容可以参考步骤103，这里就不再赘述。
请参考图4，图4为根据实际加速度和输入的期望位置，利用位置控制模型计算梯度线圈的控制电压的流程图，包括以下步骤：
步骤401：将实际加速度经二次积分计算得到胶囊内镜当前时刻的实际位置。
若控制电路的采样周期为T，则根据速度和加速度之间的关系，可以通过对加速度a一次积分得到本采样周期的速度v1，其表达式为：v1＝v0+a·T，其中，v0为上一个采样周期的速度；再根据位置和速度之间的关系，可以对速度v1再进行一次积分得到本采样周期即当前时刻的实际位置s1，其表达式为：其中，s0为上一个采样周期的位置。
步骤402：根据实际位置进行受力分析计算得到梯度线圈的平衡电流。
由于胶囊内镜还受到重力的作用，因此，当胶囊内镜处于静止状态时，仍然需要向电磁线圈通入一定大小的平衡电流，用于对胶囊内镜产生一定的电磁力与重力相平衡。
根据电磁力作用原理，胶囊内镜所受到的电磁力F(s)与胶囊内镜所处位置处的磁场强度Bc(s)以及电磁线圈的平衡电流I11之间的关系为：F(s)＝I11·(M·▽)Bc(s)，其中，M为胶囊内镜内置永磁体的磁化强度，▽为拉普拉斯算子。此外，如前所述，当电磁线圈按照一定方式设置之后，可以通过对磁场进行理论分析和建模，从而得出磁场强度Bc与空间位置s之间的变化关系，即Bc(s)，本领域技术人员可以采用不同的电磁线圈设置方式，并得到相应的Bc(s)。
令平衡电流所产生的的电磁力F(s)与胶囊内镜的重力G相平衡，即令I11·(M·▽)Bc(s)＝G，则由此可以计算出平衡电流I11。
步骤403：根据实际位置和期望位置计算得到位置误差。
根据步骤402可以计算出使胶囊内镜平衡在当前位置处的平衡电流，但是，如果要使其向指定的期望位置运动，需要额外地向电磁线圈输入调节电流I12以产生合适的驱动力。调节电流I12的计算是根据位置闭环的反馈信息进行的，若当前实际位置与理论位置存在误差，则可以通过调节电流I12对磁场进行调节，以驱动胶囊内镜向期望位置运动。若输入的期望位置为sr，则可以计算出位置误差△s：△s＝sr-s1。
步骤404：将位置误差代入位置控制模型中计算得到梯度线圈的调节电流。
将步骤403计算得到的位置误差△s代入到位置控制模型中，可以计算出梯度线圈用于驱动胶囊内镜的调节电流I12。具体地，本申请实施例所提供的控制算法是积分分离且带死区的PID控制算法。该部分的详细步骤将在图7中进行介绍。
步骤405：根据平衡电流和调节电流计算得到梯度线圈的控制电流。
梯度线圈的控制电流I1，即实际需要向梯度线圈输入的总电流，应当等于其当前位置处的平衡电流I11和调节电流I12之和，即：I1＝I11+I12。
步骤406：根据梯度线圈的控制电流和驱动梯度线圈的电阻阻值，计算得到当前时刻梯度线圈的控制电压。
若驱动梯度线圈的电阻阻值为R1，则可以得到当前时刻梯度线圈的控制电压U1为：U1＝I1·R1。
请参考图5，图5为根据实际角速度和输入的期望姿态角，利用姿态控制模型计算匀场电磁线圈的控制电压的流程图。
步骤501：根据实际加速度计算得到胶囊内镜的初始姿态角。
根据三轴加速度计测得的加速度a在坐标轴上的分量ax、ay、az，可以计算出胶囊内镜的初始姿态角Φ0(θ0,ψ0,γ0)，其中θ0为俯仰角，ψ0为偏航角、γ0为横滚角。
步骤502：根据初始姿态角和实际角速度计算得到胶囊内镜当前时刻的实际姿态角。
由测量的角速度信息来求解姿态角的过程其实就是姿态解算。在姿态解算中，常用四元数Q(q0,q1,q2,q3)来衡量和描述物体空间姿态的变化，具有简单直观的优点。如图6所示，本申请实施例所提供的姿态解算的过程包括以下步骤：
步骤5021：根据初始姿态角确定胶囊内镜初始时刻姿态的四元数。
由于每种姿态都对应一组四元数，因此由初始姿态角Φ0(θ0,ψ0,γ0)可以算出胶囊内镜初始时刻姿态的四元数Q0(q00,q10,q20,q30)。
步骤5022：根据初始时刻姿态的四元数和实际角速度，计算得到胶囊内镜当前时刻姿态的四元数。
利用三轴角速度计所测量得到的实际角速度Ω(ωx,ωy,ωz)，在初始时刻姿态的四元数Q0(q00,q10,q20,q30)的基础上对其进行更新，可以计算出当前时刻姿态的四元数Q1(q01,q11,q21,q31)。
步骤5023：根据当前时刻姿态的四元数计算得到胶囊内镜当前时刻的实际姿态角。
根据步骤5022所计算出的胶囊内镜当前时刻姿态的四元数Q1(q01,q11,q21,q31)，即可以得到当前时刻的姿态矩阵从而可以从姿态矩阵中提取得到胶囊内镜当前时刻的实际姿态角Φ1(θ1,ψ1,γ1)。
步骤503：根据实际姿态角和期望姿态角计算得到姿态角误差。
若输入的期望姿态角为Φr(θr,ψr,γr)，则可以得到当前时刻对应的姿态角误差△Φ为：△Φ＝Φr(θr,ψr,γr)-Φ1(θ1,ψ1,γ1)。
步骤504：将姿态角误差代入姿态控制模型中计算得到匀场线圈的控制电流。
将步骤503中计算得到的姿态角误差△Φ代入到姿态控制模型中，则可以计算出匀场线圈用以调整胶囊内镜姿态的控制电流I2。类似地，这里也可以采用积分分离且带死区的PID控制算法，该算法的详细步骤将在图7中进行介绍。
步骤505：根据匀场线圈的控制电流和驱动匀场线圈的电阻阻值，计算得到当前时刻匀场线圈的控制电压。
若驱动匀场线圈的电阻阻值为R2，则可以得到当前时刻匀场线圈的控制电压U2为：U2＝I2·R2。
如前所述，本申请实施例在位置控制和姿态控制中均采用了积分分离且带死区的PID控制算法，两者的算法思想和过程相同，只是算法中的具体参数不同而已。下面，将结合图7，对步骤404和步骤504中所采用的积分分离且带死区的PID控制算法进行介绍。请参考图7，图7为该算法的流程图，包括以下步骤：
步骤701：计算运动状态误差。
这里所述的运动状态误差，包括位置控制步骤403中所计算出的位置误差△s，也可以包括姿态控制步骤503中计算出的姿态角误差△Φ。
步骤702：判断运动状态误差是否小于第一门限值；若是，进入步骤703；若否，进入步骤704。
这里的第一门限值是预先设定的，可根据经验参数结合仿真调试进行选择。第一门限值的设定为了进行死区控制，所谓的死区，是指当作为被控量的运动状态误差小到一定程度时，即小于设定的第一门限值时，将控制量输出取为0，使得该段区间内控制量不产生作用。通过设置死区，可以避免控制作用过于频繁而导致的系统震荡。
当然，容易理解的是，在位置控制中和姿态控制中的第一门限值一般为不同的值，当然也可以相同，本领域技术人员可根据具体实际情况进行设置。
步骤703：将控制量取为0并输出。
若经步骤702判断当前运动状态误差小于第一门限值，则将控制量输出为0。当然，这里所述的控制量包括位置控制步骤404中的梯度线圈的调节电流I12，也可以包括姿态控制步骤504中的匀场线圈的控制电流I2。
控制量的输出将直接影响在后续步骤中对相应电磁线圈电流的调整，并最终影响到胶囊内镜的运动状态；以便于通过持续检测、计算和调整，直至胶囊内镜达到期望运动状态。
步骤704：判断运动状态误差是否小于第二门限值；若是，进入步骤705；若否，进入步骤706。
若经步骤702判断当前运动状态误差大于第一门限值，则此时需要输出一定大小的控制量用以调节和控制被控量。
考虑到当运动状态误差过大时，PID控制会引起较大超调，因此这里使用了积分分离的控制思想，即在运动状态误差过大时，取消积分作用，只进行PD控制。具体地，当前运动状态误差是否过大是通过判断其是否小于第二门限值来决定的。这里的第二门限值也是预先设定的，同样可以根据经验参数结合仿真调试结果进行选择。
容易理解的是，第二门限值应当是大于第一门限值的。类似地，位置控制中和姿态控制中的第二门限值一般为不同的取值，当然二者也可以相同，本领域技术人员可根据具体实际情况进行设置。
步骤705：根据PID控制计算控制量并输出。
若经过步骤704判断当前的运动状态误差小于第二门限值，则说明此时的运动状态比较接近输入的期望运动状态，通过PID中的积分控制，可以进一步消除静态误差，提高系统的控制精度。因此，此时采用PID控制对控制量进行计算并输出，以便在后续步骤中对相应电磁线圈的电流进行相应的调整，并持续检测、计算和调整，直至胶囊内镜达到期望运动状态。
若e表示误差，u表示控制量，则PID的计算表达式为：
其中，kp为比例系数，ki为积分系数，kd为微分系数，u0为控制量的原始初值，k＝0,1,2,…，表示第k次采样。
步骤706：根据PD控制计算控制量并输出。
若经过步骤704判断当前的运动状态误差并非小于第二门限值，则说明此时的运动状态相较于输入的期望运动状态差距较大，因此取消积分作用，以防止其带来的较大的系统超调。
若e表示误差，u表示控制量，则PD的计算表达式为：u(k)＝kp·e(k)+kd[e(k)-e(k-1)]+u0；其中，kp为比例系数，kd为微分系数，u0为控制量的原始初值，k＝0,1,2,…，表示第k次采样。
值得一提的是，在步骤705或者步骤706之后，还可以对其中的相关计算表达式中的kp等参数进行更新，以便控制系统在不同大小的运动状态误差时都能得到较好的控制效果。
下面对本申请实施例所提供的胶囊内镜运动控制系统进行介绍。下文描述的胶囊内镜运动控制系统与上文描述的胶囊内镜运动控制方法可相互对应参照。
请参阅图8，图8为本申请所提供的一种胶囊内镜运动控制系统的结构框图；包括电磁线圈801、胶囊内镜802、传感器803、驱动电路804、CPU805。
电磁线圈801用于产生磁场，对处于该磁场中的胶囊内镜802产生电磁力，以便可以调整胶囊内镜802的运动状态。
优选地，电磁线圈801可以搭配使用梯度线圈和匀场线圈，利用梯度线圈所产生的均匀场强梯度磁场和匀场线圈所产生的均匀磁场强度磁场，形成组合磁场，方便有效地对胶囊内镜802进行运动状态控制。具体地，可以通过调整梯度线圈的供电电压，对磁场的梯度进行调整，以产生合适的驱动力驱动胶囊内镜802向期望位置处运动；同时可以通过调整匀场线圈的供电电压，对磁场强度进行调整，以产生合适的磁转矩调整胶囊内镜802的姿态。
当然，电磁线圈801的个数、安装位置均可以采用不同的方案，本申请对此不做限制，本领域技术人员可以根据实际情况进行设置，并可以通过建模仿真等手段得到相应的磁场模型以及具体每个电磁线圈801对磁场的影响方式，进而确定相应的控制模型，这些也都将落在本申请保护范围之内。
胶囊内镜802是胶囊内镜运动控制系统的运动目标，用于在磁场的作用下在受检者的消化道内运动。
传感器803用于检测胶囊内镜802的实际运动状态，其内置于胶囊内镜802中。为了方便医生通过胶囊内镜802获取全面清晰的图片，传感器803可以采用三轴加速度计和三轴角速度计，以获得胶囊内镜802的实际加速度和实际角速度，以便于对其进行位置控制和姿态控制。
驱动电路804用于在CPU805的调控下向电磁线圈801提供合适的驱动电压。
CPU805用于接收传感器803所检测到的胶囊内镜802的实际运动状态，并根据该实际运动状态和输入的期望运动状态，利用控制模型计算出电磁线圈801的控制电压；并将驱动电路804为电磁线圈801提供的供电电压调整为控制电压大小，以便改变胶囊内镜802的受力情况和运动状态，直至胶囊内镜802达到期望的运动状态。
类似地，这里所述的控制模型可以包括位置控制模型和姿态控制模型；并且CPU805在利用位置控制模型和姿态控制模型进行运动状态控制时，均可以采用积分分离且带死区的PID控制算法，既可以防止在运动状态误差较小时因控制量频繁作用而产生系统震荡，也可以避免在运动状态误差较大时因积分作用而产生系统超调，从而保证较好的控制效果。
可见，本申请所提供的胶囊内镜运动控制系统，利用电磁线圈801产生磁场，因此可以通过调节电磁线圈801的供电电压有效地对磁场进行调整，从而实现对胶囊内镜802的主动控制。本申请中，具体是根据闭环反馈来计算相关的控制量的，以便可以达到消除运动状态误差，使胶囊内镜802达到指定的期望运动状态的目的。
本申请中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上对本申请所提供的胶囊内镜运动控制方法及系统进行了详细介绍。本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也落入本申请权利要求的保护范围内。