一种胶囊机器人滚转偏移角估计方法与流程

本发明属于内窥镜检查的技术领域，具体涉及一种胶囊机器人滚转偏移角估计方法。

背景技术：

内窥镜胶囊机器人小巧，不容易引起疼痛。主动内窥镜胶囊机器人在磁场控制下，具有更多的运动自由度，可覆盖肠内更多区域。操作人员根据胶囊机器人传出的图像判断胶囊相对于肠的姿态，并相应进行控制，使得胶囊机器人到达指定区域观察。

胶囊机器人在肠内经常会产生滚转运动，使得相机输出的图像也产生滚转偏移。如果不能获得胶囊机器人的滚转角度，在判断胶囊机器人与肠的相对姿态时，操作人员则无法将胶囊机器人向各方向的实际运动与根据胶囊机器人传出的图像变化推断出的期望运动进行正确对应，从而使得胶囊机器人产生错误的运动。

技术实现要素：

有鉴于此，本发明提供了一种胶囊机器人滚转偏移角估计方法，利用胶囊机器人与肠的接触力信息，能够快速获得胶囊机器人的滚转偏移角。

实现本发明的技术方案如下：

一种胶囊机器人滚转偏移角估计方法，包括以下步骤：

步骤一、标定相机正向等效点；

步骤二、在胶囊机器人的外壳沿周向设置多个力传感器，确保力传感器在胶囊机器人横截面的相邻两个投影点与横截面圆心所构成的圆心角不小于90°；

步骤三、通过力传感器测得胶囊机器人外壳所受力，并将各力感应点沿胶囊轴线投影在胶囊中心横截面a上；

步骤四、根据投影在平面a上的感应力确定等效力感应点；

步骤五、计算等效力感应点与相机正向等效点之间的圆心角，即为胶囊机器人滚转偏移角。

进一步地，步骤一具体为：找出相机输出正置图像的方向，记为相机正向，将相机安装进胶囊机器人时，如果相机中心与胶囊中心不重合，使相机正向与周向横截面上相机中心指向胶囊机器人中心的方向相同；如果相机中心与胶囊中心重合，则相机正向随意；然后，记录相机正向在胶囊机器人壳体的交点位置，此交点沿胶囊轴线方向在胶囊中心横截面a上的投影点记为相机正向等效点。

进一步地，步骤四具体为：若力感应点中只有一个感应力为有效力，则该力感应点沿胶囊轴线方向在胶囊中心横截面a上的投影点记为等效力感应点；若力感应点中有两个以上的感应力为有效力，首先把所有有效力感应点向a平面投影，再确定这些有效力投影点所覆盖的胶囊壳体部分圆弧，以距离该圆弧中心最近的投影点记为等效力感应点，所述有效力指不小于设定阈值的感应力。

有益效果：

本发明利用胶囊机器人与肠的接触力信息，从而快速计算出胶囊机器人的滚转偏移角。操作人员可以根据此偏移角和相机正向位置判断出胶囊与肠的正确相对姿态，从而正确引导胶囊机器人的下一步运动。

附图说明

图1.相机安装示意图。

图2.力感应点分布示意图一；(a)为胶囊侧视图，(b)为a横切面示意图。

图3.力感应点分布示意图二；(a)为胶囊侧视图，(b)为a横切面示意图。

图4.力感应点分布示意图三；(a)为胶囊侧视图，(b)为a横切面示意图。

图5.滚转角计算示意图。

图6.本发明方法流程图。

其中，1-胶囊机器人外壳，2-相机，3-分布在-y轴的力感应点，3’-分布在+y轴的力感应点。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

实施例1

一种胶囊机器人滚转偏移角估计方法，如图6所示，具体包括以下步骤：

步骤一，如图1、2所示，安装胶囊机器人的相机，标定相机正向等效点。

首先，找出相机输出正置图像的方向，记为相机正向。其次，将相机安装进胶囊机器人时，使相机正向与周向横截面上相机中心指向胶囊机器人中心的方向相同，如图1所示，这里相机正向为然后，记录相机正向与胶囊机器人壳体的交点n0，将此点按z轴平行方向向胶囊中心a平面投影，将此投影记为相机正向等效点，z轴方向如图2所示，其平行于胶囊的中心轴线。如图2(b)所示，n′0为相机正向等效点。

步骤二，力感应传感器安装需要下列条件：

如图2所示，(a)为胶囊侧面图，力感应传感器可以集中在a，b，c任意一个平面的胶囊外壳上，此处在a平面上，a为胶囊中心位置横切面，b、c为随机位置横切面。

图2(b)为a横切面图，其中力感应传感器随机分布，要求每个象限至少有一个力感应传感器，其个数≥4，且相邻两个力感应传感器在a横切面的投影点的圆心角应小于等于90°，此处安装8个力感应传感器，并将各个力传感器感应点进行编号，按逆时针排列，依次为n1，n2，……ni,……nk.i＝1,2,…这里的k＝8，感应点沿z轴向a平面投影，其投影点编号设为n′1，n′2……n′k，这里所有力传感器和投影点重合。

步骤三，依次测量存储力传感器沿z轴向a平面投影点。

将力传感器n1，n2，……ni,……nk点沿着z轴向a平面投影，投影点设为n′1，n′2……n′k,当力传感器在a面上时，投影点与原点重合，所有偏移角计算都在胶囊中心切面a平面上进行。

以胶囊机器人壳体a平面中心为一点，从该点指向相机正向等效点n′0的线为一边，从该点指向胶囊机器人壳体上点n′1的线为另一边，形成的角度记为相机正向等效点n′0与n′1的偏移角。

按上述方法依次测量并存储胶囊壳体上各力感应点a平面投影n′1，n′2……n′k与n′0之间的偏移角。

步骤四，确定等效力感应点。在胶囊机器人运动的过程中，力感应点中只有一个点的感应力为有效力，该力感应点ni向a平面投影点n′i记为等效力感应点。在胶囊机器人的力感应点中具有两个以上的感应力为有效力，首先把所有有效力感应点向a平面投影，再确定这些有效力的投影点所覆盖的胶囊壳体部分圆弧，以距离该圆弧的中心最近的投影点记为等效力感应点。

有效力是指不小于设定的阈值的感应力。

步骤五，计算胶囊机器人滚转偏移角。

如图5所示，获得等效力感应点后，滚转偏移角可通过步骤三中事先存储的等效力感应点n′i与胶囊机器人壳体上相机正向等效点n′0之间的圆心角获得。

实施例2

如图3所示，安装胶囊机器人各个组成部分，此处包括相机及各个力感应点传感器，力感应传感器安装如下：

3(a)为胶囊侧面图，安装的力感应传感器不是集中在a、b、c任意一个平面上，而是随机分布在胶囊的侧面上，其a横切面投影图如3(b)所示。将各个力传感器感应点进行编号，按逆时针排列，依次为n1，n2，……ni,……nk.i＝1，2，…k这里的k＝8，其相应的投影如图3(b)，依次为n′1，n′2……n′k。

其余步骤与实施例1完全相同。

实施例3

如图4所示，安装胶囊机器人各个组成部分，此处包括相机及各个力感应点传感器，力感应传感器安装如下：

4(a)为胶囊侧面图，安装的力感应传感器不是集中在a、b、c任意一个平面上，而是随机分布在胶囊的侧面上，不仅如此，在部分向a平面垂直投影直线上，分布了2个及2个以上的力感应点。如图4(a)中n1，n2，m1，m2所示。

其a横切面投影图如4(b)所示。将各个力传感器感应点进行编号，按逆时针排列，依次为n1，m1，n2，m2……ni,……nk.i＝1，2，…k这里的k＝10，其相应的投影如图4(b)，依次为n′1，n′2……n′k。这里的k＝8，其中n1，m1的投影都为n′1，n2，m2的投影都为n′2。

确定等效力感应点时，n1，m1中有不少于一个具有等效力信息，即进行等效力感应点投影计算，反之，不予处理。n2，m2的统计方法与之一致。

其余步骤与实施例1完全相同。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。