CN109770836B - 一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法 - Google Patents

Abstract

一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，属于自动化工程技术领域。该方法在利用调姿模态时双半球型胶囊机器人轴线与万向旋转磁场轴线的同轴随动特性以确定其轴线的侧摆俯仰角的基础上，通过同一俯仰角和两个不同侧摆角下单目摄像头拍摄的两幅连续图像确定摄像机框架绕双半球型胶囊机器人轴线的自转角，进而确定摄像机姿态；再利用万向旋转磁场的均匀性，结合姿态信息通过坐标转换确定在固定坐标系内弯曲肠道图像暗区质心相对于双半球型胶囊机器人的方向向量，实现万向旋转磁场对双半球型胶囊机器人在弯曲肠道内的导航。本发明无需加装任何传感器或装置，也避免使用复杂三维重建技术，即可实现双半球型胶囊机器人在肠道内的导航。

Description

一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法

技术领域

本发明属于自动化工程技术领域，涉及一种空间万向旋转磁场驱动双半球型胶囊机器人在弯曲肠道内的双图像视觉导航方法。

背景技术

人体胃肠道极易引发各种致命性疾病，其中结直肠癌是最常见病症。然而，大多数胃肠道疾病若能被早期发现和诊断，治愈率将显著提高。因此，胃肠道检查诊断在医疗领域显得十分重要。用以检查胃肠道疾病最常用的器械是传统内窥镜，由于导管可插入长度的限制，无法对整个肠道进行检查。且内窥镜导管在插入过程中，容易造成胃肠道软组织损伤，会引发患者疼痛感。

自2000年以色列Given Image公司成功开发出第一台胶囊内窥镜M2A并在次年通过美国FDA应用于临床起，就掀起了内窥镜检查技术的革命。通过吞服进入胃肠道后，能实现完整小肠的检查，避免了传统直插式内窥镜给病人带来的痛苦和不适。继M2A胶囊内窥镜之后，日本的Olympus和RF Co.Ltd,韩国的Intelligent Miro-system Center也相继推出了自己的商业化系列产品EndoCapsule、Norika以及MiroCam，我国重庆金山科技(集团)有限公司也成功推出具有完全自主知识产权的OMOM系列胶囊内窥镜，并进入临床应用。

上述胶囊内窥镜技术尽管相对成熟，但从临床应用反馈来看，仍存在诸多问题：(1)胶囊机器人主动控制问题：由于现有临床胶囊内窥镜大多不具备主动行走机构，均依靠胃肠道蠕动被动前进，因而其在胃肠道内的运动具有随机性，且胶囊错过病变区后无法返回，医生也无法对感兴趣区域进行详细细致观察，因此漏检率较高且检查效率低下。(2)胶囊机器人导航问题：由于现有胶囊内窥镜在胃肠道内的实时位置和姿态(位姿)不能确定，无法对其进行导航，从而无法实现对胶囊机器人有效的控制。这些问题均与胶囊机器人的主动导航运动控制相关，因此要实现胶囊内窥镜的高效诊疗功能，不仅要求胶囊运动行走机理安全可靠，更需要对其进行主动导航。

为实现胶囊机器人在胃肠道内的主动运动行走和非结构环境内的转弯行走难题，本课题组设计开发出双半球型胶囊机器人，并获得国家发明专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利号：CN201510262778.4)和“一种空间万向旋转磁场人机交互控制方法”(专利号：ZL201610009285.4)，已申请国家发明专利“一种双半球型胶囊机器人弯曲肠道内视觉导航方法”(申请号：201910056648.3)。

专利“一种空间万向旋转磁场人机交互控制方法”(专利号：ZL201610009285.4)中实现空间万向旋转磁场人机交互控制的具体方法是采用经纬坐标系内以旋转磁矢量侧摆角θ与俯仰角δ两个姿态角度为输入变量的电流形式的空间万向旋转磁场叠加公式，使方位控制变量由笛卡尔坐标系中的α，β，γ三个变量，经过正交变换转换为经纬坐标系中θ和δ两个变量，从而将空间万向旋转磁场的三维叠加问题转化为平面内的两维叠加问题，并通过两个操纵杆分别将侧摆与俯仰角度分离控制，在三轴亥姆霍兹线圈装置磁场均匀区域内叠加的旋转磁矢量可沿侧摆与俯仰角方向分别独立控制，实现低维度可分离变量交互旋转磁矢量控制。经纬坐标系即为固定世界坐标系，因此本文将统一称经纬坐标系为固定坐标系。

专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利号：CN201510262778.4)中提出的一种双半球型胶囊机器人的总体结构为：双半球型胶囊机器人结构包括主动半球体和被动半球体两部分，将径向磁化钕铁硼圆环内驱动器7和摄像机与图像传输装置8过盈装配，将阶梯轴6也与摄像机与图像传输装置8过盈装配，最后将摄像机与图像传输装置8组件再与主动半球壳1过盈配合构成主动半球体；轴承定位套筒3与被动半球壳2过盈配合构成被动半球体，主动半球体和被动半球体由轴承4悬浮连接的过程如下：将轴承4安装在主动半球体组件的阶梯轴6上，再将主动半球体组件阶梯轴6上的轴承4一并装入轴承定位套筒3中，轴承定位套筒3内部有一台阶实现轴承4外圈轴向定位，圆螺母5装入阶梯轴6上以将轴承4内圈轴向定位，圆螺母5不能突出到球面以外，以防止主动半球转动过程中带动圆螺母5与肠道接触影响姿态调整。双半球型胶囊机器人的工作原理为：旋转磁场与径向磁化钕铁硼圆环内驱动器7的耦合磁矩带动包括摄像机与图像传输装置8的主动半球体绕被动半球体相对空转，主动半球体处于驱动状态，被动半球体在配重作用下始终处于下方，处于欠驱动状态，与胃肠道接触的摩擦力约束下的被动半球体处于静止状态，可防止双半球型胶囊机器人调姿时发生滚动，主动半球处于上方不与胃肠道接触或者与胃肠道接触区域较小，主动半球体相对位于下面静止的被动半球体空转，摄像机与图像传输装置8中的摄像机光轴与双半球型胶囊机器人轴线一致，调整姿态时在胃肠道接触面上方施加旋转磁矢量，主动半球体相对被动半球体空转，随动效应使双半球型胶囊机器人轴线一直追随相应方位角旋转磁场的轴线实现胃肠道内的姿态任意调整。该专利提出的双半球型胶囊机器人的导航过程为：将三组线圈相互正交嵌套安装成三轴正交嵌套亥姆霍兹线圈磁场叠加装置c，让患者a吞下双半球型胶囊机器人d，并躺在病床b上，调整病床b的位置使患者d处于三轴正交嵌套亥姆霍兹线圈磁场叠加装置c的中心区域。当双半球型胶囊机器人处于位置A时，根据双半球型胶囊机器人轴线与磁矢量方向始终一致的随动效应原理，调整磁矢量方向使无线传输图像对准肠道弯曲方向，此时磁矢量方向与肠道弯曲方向一致，并在水平面内施加与肠道弯曲方向垂直的旋转磁矢量，从而驱动双半球型胶囊机器人滚动到位置B，再重复以上过程运动到位置C，以此类推。

尽管该两项发明专利给出了双半球型胶囊机器人的弯曲肠道控制方法，但该两项发明并没有给出如何确定磁矢量方向使无线传输图像对准肠道弯曲方向的方法，即没有给出具体的导航方向确定方法。

若要确定导航方向，必须要先求得当前双半球型胶囊机器人的姿态信息。目前普遍的视觉导航都采用多目视觉系统，然而双半球型胶囊机器人内部空间狭小，安装多个摄像机的方法会受双半球型胶囊机器人空间的限制、需要额外的电路传输信息且对多目镜头的安装精度有较高的要求，从而造成双半球型胶囊机器人内部结构过于复杂，且复杂电路会造成系统可靠性降低。三维重建技术虽然可以计算当前双半球型胶囊机器人位置，但算法复杂，对图像参数要求较高。

因此，本课题组在以上两个发明专利的基础上，申请了专利“一种双半球型胶囊机器人弯曲肠道内视觉导航方法”(申请号：201910056648.3)，提出一种双半球型胶囊机器人弯曲肠道内视觉导航方法：利用双半球型胶囊机器人在旋转磁场中的随动效应，可知双半球型胶囊机器人轴线始终与旋转磁矢量方向一致，即摄像机的光轴方向也与旋转磁矢量方向一致，即姿态调整后静止的双半球型胶囊机器人摄像机的光轴方向可用固定坐标系内侧摆角θ与俯仰角δ两个姿态角描述，但摄像机绕双半球型胶囊机器人轴线的自转角度未知，并具有随机性，因此无法确定摄像机姿态。该专利提出采用一种加装在双半球型胶囊机器人内的铅垂角度传感器，在重力作用下，通过铅垂角度传感器得到摄像机绕双半球型胶囊机器人轴线的自转角度。然后再提取摄像机拍摄弯曲肠道时所发射无线传输图像的暗区质心像素位置，以此作为肠道转弯导航点，结合摄像机的姿态信息通过坐标转换确定在固定坐标系内弯曲肠道图像上暗区质心相对于双半球型胶囊机器人的方向向量，实现空间万向磁场对双半球型胶囊机器人的转弯导航。

虽然专利“一种双半球型胶囊机器人弯曲肠道内视觉导航方法”(申请号：201910056648.3)提出的导航方法可以用于双半球型胶囊机器人的导航，但在双半球型胶囊机器人内安装铅垂角度传感器的方式会对双半球型胶囊机器人的体积控制有影响，会提高双半球型胶囊机器人内部安装精度要求和加工精度要求，而且对操作要求较高。

为了进一步缩小双半球型胶囊机器人的体积，降低安装精度和加工精度并简化双半球型胶囊机器人内部结构，本课题组在专利“一种双半球型胶囊机器人弯曲肠道内视觉导航方法”(申请号：201910056648.3)的基础上，结合双半球型胶囊机器人结构特征和人机交互控制方法，在不需要在双半球型胶囊机器人内加装其它装置的条件下，提出一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法。该方法首先利用双半球型胶囊机器人轴线与万向磁场旋转轴线的同轴随动特性，即双半球型胶囊机器人轴线、摄像机光轴和万向磁场旋转轴线都重合，结合同一俯仰角和两个不同侧摆角下单目摄像机所拍摄的两幅无线传输图像确定双半球型胶囊机器人的姿态信息；然后结合得到的姿态信息，在万向均匀旋转磁场条件下推导双半球型胶囊机器人的视觉导航方位，即通过计算固定坐标系下弯曲肠道图像暗区质心的方位来调整双半球型胶囊机器人姿态准确对准肠道弯曲方向，并确定双半球型胶囊机器人转弯滚动磁场轴线方向，实现双半球型胶囊机器人在肠道内的视觉辅助导航作业。

目前，尚未有人提出利用万向旋转磁场的均匀性和双半球型胶囊机器人在磁场内的随动性，在无须在双半球型胶囊机器人内加装任何传感器或其它装置的条件下，通过两幅图像来确定双半球型胶囊机器人摄像机相对固定坐标系的姿态信息并通过坐标变换确定导航方向的方法，该方法的显著优点是不需要在双半球型胶囊机器人内加装其它装置，也不需要使用多目视觉系统和使用复杂肠道三维影像重构技术，依靠双半球型胶囊机器人的单目摄像机视觉并结合万向旋转磁场的均匀性、双半球型胶囊机器人在万向旋转磁场中的随动性，通过坐标转换，得到双半球型胶囊机器人的最终导航信息。

发明内容

本发明提供一种以空间万向旋转磁场驱动实现双半球型胶囊机器人在弯曲肠道内的双图像视觉导航方法，即在以双半球型胶囊机器人在万向旋转磁场中的随动特性确定双半球型胶囊机器人轴线的侧摆与俯仰姿态角度的基础上，通过同一俯仰角和两个不同侧摆角下摄像机拍摄的两幅图像，确定摄像机框架绕双半球型胶囊机器人轴线的自转角度，进而确定摄像机相对于固定坐标系的姿态，最终确定在固定坐标系内弯曲肠道图像上暗区质心相对于双半球型胶囊机器人的方向向量，实现在弯曲肠道内对双半球型胶囊机器人的导航。

本发明的技术方案为：

一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，步骤如下：

第一步：当双半球型胶囊机器人在当前位置上需要进行导航时，通过同一俯仰角和两个不同侧摆角下摄像机拍摄的连续两幅图像，即双半球型胶囊机器人在当前位置上得到一幅图像后，保持其轴线方向的俯仰角度不变，改变其轴线的侧摆角度，得到第二幅图像，通过两幅连续图像确定摄像机框架绕双半球型胶囊机器人轴线的自转角度α*。

技术方案第一步中，所述的摄像机框架绕双半球型胶囊机器人轴线的自转角度α*确定方法为：当双半球型胶囊机器人调姿完成处于静止时，此时得到像平面V₁，像平面V₁上有肠道暗区质心点m₁；保持磁矢量方向的俯仰角不变，只改变侧摆角，双半球型胶囊机器人再次处于静止，此时得到像平面V₂，像平面V₂上有肠道暗区质心点m₂。侧摆过程中主动半球空转，因此摄像机框架绕双半球型胶囊机器人轴线的自转角保持不变。设操控侧摆角逆时针变化为正，从摄像机光心沿摄像机光轴方向观察像平面V₁和像平面V₂，记像平面V₁和像平面V₂绕摄像机光轴顺时针旋转为正。当操控侧摆角顺时针变化，情况与侧摆角逆时针变化完全相反，其中，侧摆角逆时针变化时：

1)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转0～90°时，将肠道暗区质心点m₂的像素坐标赋予像平面V₁上的对应点m₂’，m₂’m₁的连线平行于水平面，过肠道暗区质心点m₁作直线平行于像平面上边缘线，摄像机框架绕双半球型胶囊机器人轴线的自转角α^*即为该边缘线与m₂’m₁的连线所夹锐角α。

2)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转0～-90°时，摄像机框架绕双半球型胶囊机器人轴线的自转角α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝-α；

3)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转90～180°时，摄像机框架绕双半球型胶囊机器人轴线的自转角α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝90+90-α＝180-α；

4)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转-90～-180°时，摄像机框架绕双半球型胶囊机器人轴线的自转角α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝-(90+90-α)＝α-180；

操控侧摆角顺时针或逆时针变化，按自转角度分成以上四种情况，通过肠道暗区质心点m₁和肠道暗区质心点m₂的像素坐标代数关系判断此时像平面的所属情形。进而摄像机框架绕双半球型胶囊机器人轴线的自转角度α*的计算方式为：

当操控侧摆角逆时针变化时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α*为：

当操控侧摆角顺时针变化时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α*为:

其中

其中，(x_m1,y_m1)、(x_m2,y_m2)分别为肠道暗区质心点m₁和肠道暗区质心点m₂的像素横纵坐标。

第二步：结合得到第一幅图像后双半球型胶囊机器人轴线的俯仰角和侧摆角信息以及摄像机绕双半球型胶囊机器人轴线的自转角度α*，确定摄像机相对于固定坐标系的姿态。

技术方案第二步中，所述的确定摄像机相对于固定坐标系的姿态，具体确定方法为：利用双半球型胶囊机器人在旋转磁场中的随动效应，得知双半球型胶囊机器人的轴线方向始终与旋转磁矢量方向一致，因此，双半球型胶囊机器人摄像机的光轴方向即为固定坐标系内侧摆角θ与俯仰角δ两个姿态角；结合第一步中确定的摄像机框架绕双半球型胶囊机器人轴线的自转角α*，即得到双半球型胶囊机器人相对于固定坐标系的姿态信息。

第三步：用图像处理技术提取拐弯处的暗区质心像素坐标，以此作为肠道转弯导航点。由于双半球型胶囊机器人始终处于三轴亥姆霍兹线圈磁场均匀区域，通过坐标变换，将图像暗区质心的像素坐标转化为在固定坐标系内质心相对于双半球型胶囊机器人的方向向量，再由双半球型胶囊机器人运动原理所要求的施加滚动磁场磁矢量方向需与水平面平行，从而计算出从当前位置运动到目标位置需要施加的磁矢量方向，以此来实现双半球型胶囊机器人的导航。

技术方案第三步中，双半球型胶囊机器人导航坐标转化方法及计算公式具体为：当双半球型胶囊机器人进入工作环境时，需获知其在体内的位置及姿态。由于三轴亥姆霍兹线圈产生的旋转磁场具有区域均匀性及各点同向性，所以在磁场均匀区内，无论双半球型胶囊机器人处于何种位置，其轴线方向始终与磁矢量方向一致，因此将固定坐标系OXYZ的原点O平移至摄像机的光心O_C，坐标轴方向不变，构成动坐标系O_CX_WY_WZ_W，所要求解的固定坐标系内导航点相对于双半球型胶囊机器人的方向向量等价于导航点在动坐标系O_CX_WY_WZ_W内相对于O_C的方向向量。计算出导航点在动坐标系中相对于原点的方向向量，此向量即导航方向。故只需根据公式(1)和(2)求得双半球型胶囊机器人的姿态信息，并不需要获知双半球型胶囊机器人的位置信息即可导航。

建立坐标系：建立的坐标系包含固定坐标系OXYZ、动坐标系O_CX_WY_WZ_W、双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁、摄像机轴线坐标系O_CX_CY_CZ_C、图像像素坐标系O₀uv和图像物理坐标系O₁xy。图像像素坐标系上的暗区质心坐标点p的坐标为(u,v)，目标导航点P是与暗区质心坐标点p对应的物点。所要求的最终导航方向即目标导航点P在动坐标系中相对于原点的方向向量Ο_CP与O_Cp的方向一致，因此确定导航方向时，需将图像像素坐标系内的暗区质心坐标点p的坐标为(u,v)转化成动坐标系O_CX_WY_WZ_W内O_Cp的方向向量，

动坐标系O_CX_WY_WZ_W和双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁的变换关系为：将动坐标系先绕O_CZ_W轴顺时针旋转θ度，再绕O_CX_W轴逆时针旋转δ度，得到双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁，此时双半球型胶囊机器人轴线方向与O_CZ₁轴重合；

双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁绕O_CZ₁轴顺时针旋转α^*角度，得到摄像机轴线坐标系O_CX_CY_CZ_C；

图像物理坐标系O₁xy垂直于O_CZ_C轴，原点O₁在O_CZ_C轴上，O₁O_C的长度为摄像机焦距f，且O₁x轴与O_CZ_C轴平行，O₁y轴与O_CY_C轴平行；

图像物理坐标系原点O₁向y轴负方向平移v₀，再向x轴负方向平移u₀，得到图像像素坐标系O₀uv。

坐标变换过程为：先将暗区质心坐标点p的图像像素坐标(u,v)转换成图像物理坐标(x,y)：

经过转换得到：

求逆后就得到图像物理坐标和图像像素坐标系之间的关系：

在摄像机轴线坐标系O_CX_CY_CZ_C中，暗区质心坐标点p的Z_C坐标值为摄像机的焦距f，因此暗区质心坐标点p在摄像机轴线坐标系O_CX_CY_CZ_C中的坐标为(x,y,f)。因此式(7)表示为

其中，dx，dy，u₀，v₀均为摄像机内部参数，每个像素的物理尺寸大小为dx*dy(mm)，u₀为图像物理坐标系O₁xy原点O₁相对于计算机图像坐标系O₀uv原点O₀在O₁x轴方向上的偏移量，v₀为图像物理坐标系O₁xy原点O₁相对于计算机图像坐标系O₀uv原点O₀在O₁y轴方向上的偏移量，f为摄像机焦距。

摄像机轴线坐标系内的点(x,y,f)转换到双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁的式子为：

逆运算得：

最后将(X₁,Y₁,f)转换到动坐标系O_CX_WY_WZ_W中：正交变换矩阵为

则

得到

结合以上坐标变换，即可计算图像暗区质心在动坐标系O_CX_WY_WZ_W内的方向n_S：

其中

再通过专利“一种空间万向旋转磁场人机交互控制方法”(专利号：ZL201610009285.4)提出的正交变换方法，将n_S变换为用θ和δ两个变量表示，即得到了最终导航方向。

所述的正交变换方法为：先将向量n_S投影到X_WO_CY_W面内，此投影与O_CX_W轴的夹角为侧摆角θ；再将向量n_S投影到X_WO_CZ_W面内，此投影与O_CZ_W轴的夹角为俯仰角δ。

由于滚动磁场需在水平面内施加，因此，在求得n_S并变换为用θ和δ两个变量表示后，使其δ值为0，即得到了导航应施加的滚动磁场，由此完成第三步所述双半球型胶囊机器人的导航。

本发明的有益效果：在双半球型胶囊机器人内部有限的空间条件下，不需要在双半球型胶囊机器人内部加装传感器或其它辅助装置，且不用计算双半球型胶囊机器人在固定坐标系内的具体位置信息，利用万向旋转磁场的区域均匀特性、各点同向特性和双半球型胶囊机器人与万向旋转磁场的轴线同向性，通过双半球型胶囊机器人在同一俯仰角不同侧摆角情况下拍摄的两幅图像确定其姿态信息，进而确定双半球型胶囊机器人的导航方向，避免使用复杂多目摄像机结构和复杂肠道三维影像重构技术，以及复杂的传感器传输电路和其它辅助装置，实现了通过单目视觉辅助双半球型胶囊机器人在弯曲肠道内的导航作业，结构简单可靠，降低安装及加工精度要求，占用空间小，操作过程简单快捷，人机交互操作方便，提高了双半球型胶囊机器人在弯曲肠道内的可操作性。

附图说明

图1是双半球型胶囊机器人人机交互导航控制的技术方案示意图。

图2(a)是双半球形胶囊机器人外部结构局部放大图。

图2(b)是双半球型胶囊机器人内部结构局部放大图。

图3是双半球型胶囊机器人导航过程示意图。

图4是前后连续两幅图像关系示意图。

图5(a)表示像平面绕双半球型胶囊机器人轴线旋转0～90°时α与α*关系示意图；图5(b)表示像平面绕双半球型胶囊机器人轴线旋转0～-90°时α与α*关系示意图；图5(c)表示像平面绕双半球型胶囊机器人轴线旋转90～180°时α与α*关系示意图；图5(d)表示像平面绕双半球型胶囊机器人轴线旋转-90～-180°时α与α*关系示意图。

图6是固定坐标系OXYZ和动坐标系O_CX_WY_WZ_W转换关系示意图。

图7是所有坐标系转换关系总图

图8是动坐标系O_CX_WY_WZ_W和双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁转换关系示意图

图9是图像像素坐标系O₀uv和图像物理坐标系O₁xy转换关系示意图

图10(a)是拍摄的第一幅图像暗区质心像素坐标示例图。

图10(b)是拍摄的第二幅图像暗区质心像素坐标示例图。

图中：a患者；b病床；c三轴正交嵌套亥姆霍兹线圈磁场叠加装置；d双半球型胶囊机器人；e1侧摆操纵杆；e2俯仰操纵杆；f信号处理器；g外磁场驱动器；h人机交互界面；p暗区质心坐标点；m肠道暗区质心点；n调姿磁场旋转轴线。A位置A；B位置B；C位置C；V像平面；P目标导航点；1主动半球壳；2被动半球壳；3轴承定位套筒；4轴承；5圆螺母；6阶梯轴；7径向磁化钕铁硼圆环内驱动器；8摄像机与图像传输装置；9摄像机光心；10摄像机光轴。

具体实施方式

以下结合技术方案和附图3、图10(a)、图10(b)详细叙述本发明的实施步骤和具体实施例。

实施步骤：

第一步：当胶囊调姿完成处于静止时，拍摄第一幅图像，提取图像暗区质心像素位置，读取此时磁矢量方向(θ₁,δ₁)；保持磁矢量方向的俯仰角不变，只改变侧摆角，胶囊再次处于静止，拍摄第二幅图像，提取图像暗区质心像素位置，读取此时磁矢量方向(θ₂,δ₂)，坐标值取值在(0,0)到(640,480)之间。利用两幅图像的暗区质心位置作为特征点，通过式子(1)-(3)计算α*，其取值范围为-180～180°。

第二步：读取此时双半球型胶囊机器人的轴线方向即此时旋转磁矢量的方向θ、δ。

第三步：通过第一幅肠道图像或第二幅肠道图像，结合其姿态信息，将已知的参数代入式(15)-(18)，计算得暗区质心相对于双半球型胶囊机器人的方向矢量n_S，再计算下一步应施加的滚动磁矢量方向。

具体实施例：

(1)当双半球型胶囊机器人在位置A，如图3所示，拍摄第一幅图像，提取暗区质心像素坐标为(520，135)，如图10(a)所示，从磁场控制器读取此时磁矢量方向θ＝50°和δ＝75°；保持磁矢量方向的俯仰角不变，只改变侧摆角，双半球型胶囊机器人再次处于静止，拍摄第二幅图像，提取图像暗区质心像素坐标为(361，476)，如图10(b)所示，从磁场控制器读取此时磁矢量方向θ＝30°和δ＝75°。通过式子(1)-(3)计算摄像机绕双半球型胶囊机器人轴线的自转角度α*＝65°。

(2)使用拍摄第一幅图像时的磁矢量方向θ＝50°和δ＝75°，结合摄像机绕双半球型胶囊机器人轴线的自转角度α*＝65°，即可确定当前双半球型胶囊机器人相对于固定坐标系的姿态信息。

(3)根据摄像机拍摄的第一幅图像，以暗区质心作为肠道转弯导航点，暗区质心像素坐标为(520，135)。将已知的α*＝65°、θ＝50°、δ＝75°和暗区质心像素坐标(520，135)代入式(15)-(18)中，计算暗区质心在动坐标系内的方向矢量n_S＝(1.6,1,1.6)，通过旋转磁场正交变换公式将n_S用θ和δ两个变量表示，计算得θ_S＝45°，δ_S＝30°，由此即确定了双半球型胶囊机器人导航方向。由于滚动磁场需在水平面内施加，因此，将δ设为0，则下一步应施加的滚动磁矢量方向为θ＝45°，δ＝0。

Claims (5)

1.一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，其特征在于，步骤如下：

第一步：当双半球型胶囊机器人在当前位置上需要进行导航时，通过同一俯仰角和两个不同侧摆角下摄像机拍摄的连续两幅图像，即双半球型胶囊机器人在当前位置上得到一幅图像后，保持其轴线方向的俯仰角度不变，改变其轴线的侧摆角度，得到第二幅图像，通过两幅连续图像确定摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*；

第二步：结合得到第一幅图像后双半球型胶囊机器人轴线的俯仰角和侧摆角信息以及摄像机框架绕双半球型胶囊机器人轴线的自转角度α*，确定摄像机相对于固定坐标系的姿态；

第三步：用图像处理技术提取拐弯处的暗区质心像素坐标，以此作为肠道转弯导航点；由于双半球型胶囊机器人始终处于三轴亥姆霍兹线圈磁场均匀区域，通过坐标变换，将图像暗区质心的像素坐标转化为在固定坐标系内质心相对于双半球型胶囊机器人的方向向量，再由双半球型胶囊机器人运动原理所要求的施加滚动磁场磁矢量方向需与水平面平行，从而计算出从当前位置运动到目标位置需要施加的磁矢量方向，以此来实现双半球型胶囊机器人的导航；

双半球型胶囊机器人导航坐标转化方法及计算公式具体为：当双半球型胶囊机器人进入工作环境时，需获知其在体内的位置及姿态；由于三轴亥姆霍兹线圈产生的旋转磁场具有区域均匀性及各点同向性，所以在磁场均匀区内，无论双半球型胶囊机器人处于何种位置，其轴线方向始终与磁矢量方向一致，因此将固定坐标系OXYZ的原点O平移至摄像机的光心O_C，坐标轴方向不变，构成动坐标系O_CX_WY_WZ_W，所要求解的固定坐标系内导航点相对于双半球型胶囊机器人的方向向量等价于导航点在动坐标系O_CX_WY_WZ_W内相对于O_C的方向向量；计算出导航点在动坐标系中相对于原点的方向向量，此向量即导航方向；故只需结合第一步中确定的摄像机框架绕双半球型胶囊机器人轴线的自转角度α*，求得双半球型胶囊机器人的姿态信息，并不需要获知双半球型胶囊机器人的位置信息即可导航；

建立坐标系：建立的坐标系包含固定坐标系OXYZ、动坐标系O_CX_WY_WZ_W、双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁、摄像机轴线坐标系O_CX_CY_CZ_C、图像像素坐标系O₀uv和图像物理坐标系O₁xy；图像像素坐标系上的暗区质心坐标点p的坐标为(u,v)，目标导航点P是与暗区质心坐标点p对应的物点；所要求的最终导航方向即目标导航点P在动坐标系中相对于原点的方向向量Ο_CP与O_Cp的方向一致，因此确定导航方向时，需将图像像素坐标系内的暗区质心坐标点p的坐标为(u,v)转化成动坐标系O_CX_WY_WZ_W内O_Cp的方向向量；

动坐标系O_CX_WY_WZ_W和双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁的变换关系为：将动坐标系先绕O_CZ_W轴顺时针旋转θ度，再绕O_CX_W轴逆时针旋转δ度，得到双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁，此时双半球型胶囊机器人轴线方向与O_CZ₁轴重合；

双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁绕O_CZ₁轴顺时针旋转α^*角度，得到摄像机轴线坐标系O_CX_CY_CZ_C；

图像物理坐标系O₁xy垂直于O_CZ_C轴，原点O₁在O_CZ_C轴上，O₁O_C的长度为摄像机焦距f，且O₁x轴与O_CZ_C轴平行，O₁y轴与O_CY_C轴平行；

图像物理坐标系原点O₁向y轴负方向平移v₀，再向x轴负方向平移u₀，得到图像像素坐标系O₀uv；

坐标变换过程为：先将暗区质心坐标点p的图像像素坐标(u,v)转换成图像物理坐标(x,y)：

经过转换得到：

求逆后就得到图像物理坐标和图像像素坐标系之间的关系：

在摄像机轴线坐标系O_CX_CY_CZ_C中，暗区质心坐标点p的Z_C坐标值为摄像机的焦距f，因此暗区质心坐标点p在摄像机轴线坐标系O_CX_CY_CZ_C中的坐标为(x,y,f)；因此式(4 )表示为

其中，dx，dy，u₀，v₀均为摄像机内部参数，每个像素的物理尺寸大小为dx*dy(mm)，u₀为图像物理坐标系O₁xy原点O₁相对于图像像素坐标系O₀uv原点O₀在O₁x轴方向上的偏移量，v₀为图像物理坐标系O₁xy原点O₁相对于图像像素坐标系O₀uv原点O₀在O₁y轴方向上的偏移量，f为摄像机焦距；

摄像机轴线坐标系内的点(x,y,f)转换到双半球型胶囊机器人轴线坐标系O_CX₁Y₁Z₁的式子为：

逆运算得：

最后将(X₁,Y₁,f)转换到动坐标系O_CX_WY_WZ_W中：正交变换矩阵为

则

得到

结合以上坐标变换，即可计算图像暗区质心在动坐标系O_CX_WY_WZ_W内的方向n_S：

其中

其中，(x_m1,y_m1)、(x_m2,y_m2)分别为肠道暗区质心点m₁和肠道暗区质心点m₂的像素横纵坐标；

再通过正交变换方法，将n_S变换为用θ和δ两个变量表示，即得到了最终导航方向。

2.根据权利要求1所述的一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，其特征在于，第一步中，所述的摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*确定方法为：当双半球型胶囊机器人调姿完成处于静止时，此时得到像平面V₁，像平面V₁上有肠道暗区质心点m₁；保持磁矢量方向的俯仰角不变，只改变侧摆角，双半球型胶囊机器人再次处于静止，此时得到像平面V₂，像平面V₂上有肠道暗区质心点m₂；侧摆过程中主动半球空转，因此摄像机框架绕双半球型胶囊机器人轴线的自转角保持不变；设操控侧摆角逆时针变化为正，从摄像机光心沿摄像机光轴方向观察像平面V₁和像平面V₂，记像平面V₁和像平面V₂绕摄像机光轴顺时针旋转为正；当操控侧摆角顺时针变化，情况与侧摆角逆时针变化完全相反，其中，侧摆角逆时针变化时：

1)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转0～90°时，将肠道暗区质心点m₂的像素坐标赋予像平面V₁上的对应点m₂’，m₂’m₁的连线平行于水平面，过肠道暗区质心点m₁作直线平行于像平面上边缘线，摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*即为该边缘线与m₂’m₁的连线所夹锐角α；

2)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转0～-90°时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝-α；

3)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转90～180°时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝90+90-α＝180-α；

4)当操控侧摆角逆时针变化，像平面绕双半球型胶囊机器人轴线旋转-90～-180°时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α^*和图像V₁上m₂’m₁的连线与过m₁平行于像平面上边缘线的直线所夹锐角α的关系为α*＝-(90+90-α)＝α-180°；

操控侧摆角顺时针或逆时针变化，按自转角度分成以上四种情况，通过肠道暗区质心点m₁和肠道暗区质心点m₂的像素坐标代数关系判断此时像平面的所属情形；进而摄像机框架绕双半球型胶囊机器人轴线的自转角度α*的计算方式为：

当操控侧摆角逆时针变化时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α*为：

当操控侧摆角顺时针变化时，摄像机框架绕双半球型胶囊机器人轴线的自转角度α*为:

其中

其中，(x_m1,y_m1)、(x_m2,y_m2)分别为肠道暗区质心点m₁和肠道暗区质心点m₂的像素横纵坐标。

3.根据权利要求1或2所述的一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，其特征在于，第二步中，所述的确定摄像机相对于固定坐标系的姿态，具体确定方法为：利用双半球型胶囊机器人在旋转磁场中的随动效应，得知双半球型胶囊机器人的轴线方向始终与旋转磁矢量方向一致，因此，双半球型胶囊机器人摄像机的光轴方向即为固定坐标系内侧摆角θ与俯仰角δ两个姿态角；结合第一步中确定的摄像机框架绕双半球型胶囊机器人轴线的自转角度α*，即得到双半球型胶囊机器人相对于固定坐标系的姿态信息。

4.根据权利要求1或2所述的一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，其特征在于，所述的正交变换方法为：先将向量n_S投影到X_WO_CY_W面内，此投影与O_CX_W轴的夹角为侧摆角θ；再将向量n_S投影到X_WO_CZ_W面内，此投影与O_CZ_W轴的夹角为俯仰角δ。

5.根据权利要求3所述的一种双半球型胶囊机器人弯曲肠道内双图像视觉导航方法，其特征在于，所述的正交变换方法为：先将向量n_S投影到X_WO_CY_W面内，此投影与O_CX_W轴的夹角为侧摆角θ；再将向量n_S投影到X_WO_CZ_W面内，此投影与O_CZ_W轴的夹角为俯仰角δ。

Images