CN109766784A - 基于单目图像的胶囊机器人交互控制方法 - Google Patents

Abstract

本发明提出一种基于单目图像的胶囊机器人交互控制方法，能够使得胶囊机器人的运动方向沿着胃肠孔腔的弯曲方向前进。该方法通过胶囊机器人前端的单目相机采集胃肠图像，对所述胃肠图像进行处理，提取胃肠结构的褶皱轮廓信息，重建出胃肠局部三维结构；将所述胃肠局部三维结构结合位姿检测机构提供的相机位姿信息进行交互控制来调整胶囊机器人的前进方向。

Description

基于单目图像的胶囊机器人交互控制方法

技术领域

本发明属于图像处理的交互控制领域，尤其涉及一种基于单目图像的胶囊机器人交互控制方法。

背景技术

胶囊机器人在胃肠检查过程中，利用前端的单目相机采集胃肠环境图像，根据图像中的孔腔信息来指导胶囊机器人的前进方向。由于肠道结构的复杂性，在实际的操作中经常会遇到胶囊机器人顶在肠壁上或是被肠道褶皱遮挡，这会导致图像中没有可用信息，丢失胶囊机器人正确的前进方向。

目前，在这种情况下，主要依靠人为的经验或是随机的调整运动方向以摆脱可视信息丢失的情况，难以实现快速准确的姿态控制。

发明内容

本发明提出一种基于单目图像的胶囊机器人交互控制方法，能够使得胶囊机器人的运动方向沿着胃肠孔腔的弯曲方向前进。

本发明通过以下技术方案实现：

一种基于单目图像的胶囊机器人交互控制方法，通过胶囊机器人前端的单目相机采集胃肠图像，对所述胃肠图像进行处理，提取胃肠结构的褶皱轮廓信息，重建出胃肠局部三维结构；将所述胃肠局部三维结构结合相机位姿信息来调整胶囊机器人的前进方向。

进一步地，对胶囊机器人采集的胃肠图像进行分块处理，对每个子图像块进行滤波和特征提取。

进一步地，所述胃肠图像进行分块采用以下方式，胃肠图像的分辨率为M*N,图像的行和列方向上分别都分成n段(n为奇数)，形成n*n个分辨率为M/n*N/n的图像子块。

进一步地，所述单目相机采集胃肠图像后，通过图像平均灰度信息判断是否找到肠道孔腔，如果图像中包括孔腔信息则利用相机内参数构造成像模型，得到孔腔方向的三维向量并计算调整胶囊机器人的偏航角和俯仰角，使得孔腔区域落在图像中间区域。

进一步地，所述单目相机采集胃肠图像后，通过图像平均灰度信息判断是否找到肠道孔腔，如果图像中不包括孔腔信息则控制胶囊机器人分别以偏航角±δ₀和俯仰角±δ₀向四个方向进行旋转，再通过计算相应旋转后采集图像的各子块平均灰度信息，找到平均灰度最小的子块为目标区域，利用相机内参数构造成像模型，得到目标区域方向的三维向量并计算调整胶囊机器人的偏航角和俯仰角，使得目标区域落在图像中间区域，重复此过程直至在图像中找到孔腔区域。

进一步地，所述重建出胃肠局部三维结构采用以下方式：根据所述成像模型构造反向映射模型，将采集到的胃肠图像中二维褶皱轮廓信息转换到三维空间，并根据Lambertian反射定理计算出深度信息，重建出胃肠局部的三维结构。

进一步地，所述二维褶皱轮廓信息采用以下方式获得：对肠道孔腔的图像进行预处理滤除噪声，并采用Canny算法进行边缘检测提取出非连续的轮廓线段；通过形态学操作去除掉噪声引起的假边缘，得到候选轮廓集合A，保留轮廓线集合中属于同一褶皱的轮廓做平滑连接。

进一步地，判断所述集合A中属于同一褶皱的方法如下：在轮廓线集合A中找到端点间距离最小的两段轮廓线，然后计算两端点分别与孔腔中心点连接所形成的夹角、两端点连线分别与两段轮廓线所选端点的切线形成的夹角，如果第一个夹角角度小于θ₀度，并且第二、三个夹角角度同时小于θ₁度，那么就认为这两段轮廓线属于同一褶皱。

进一步地，以相邻褶皱间的中心连线作为规划路径，计算出胶囊机器人的调整方向和距离。

进一步地，当胶囊机器人在行进过程中，通过定位模块计算出在世界坐标系下的相机位姿，利用世界坐标系下重建局部模型中相机所在位置，找到距离最近的前后两个闭合褶皱轮廓的中心点，连接两个中心点形成在三维空间中的前进轨迹矢量，计算出相机到直线的距离d和方向n，控制胶囊机器人沿着n方向移动距离d，使得胶囊机器人回到规划的运动轨迹上。

进一步地，所述单目相机固连在胶囊机器人上。

本发明的有益效果：

采用本发明的交互控制方法，通过胶囊机器人前端的单目相机采集胃肠图像，对图像进行处理提取胃肠结构的褶皱轮廓信息，重建出胃肠局部三维结构；结合位姿检测机构提供的相机位姿信息进行交互控制，使得胶囊机器人的运动方向能够沿着胃肠孔腔的弯曲方向前进。

附图说明

图1为本发明胶囊机器人的姿态调整示意图；

图2为本发明中将二维的褶皱轮廓反向映射到三维空间坐标图；

图3为本发明胶囊机器人前进示意图。

具体实施方式

下面结合附图对本发明进行进一步描述。

本发明提供一种主动胶囊机器人胃肠检查过程中，通过胶囊机器人前端的单目相机采集胃肠图像，对所述胃肠图像进行处理，提取胃肠结构的褶皱轮廓信息，重建出胃肠局部三维结构；将所述胃肠局部三维结构结合相机位姿信息来调整胶囊机器人的前进方向。具体步骤如下：

步骤一，对胶囊机器人采集图像进行分块处理，对每个子图像块进行滤波和特征提取得到方向信息，使得胶囊机器人能够沿着肠道孔腔的方向行进。

本步骤的主要执行过程如下：胶囊机器人采集图像分辨率为960*720，在图像的行和列方向上分别都分成三段，形成9个分辨率为320*240的图像子块，即中心子块和周围八个方向上的子块；计算每个子块的RGB通道图像的平均灰度，找出满足以下条件：RGB三个通道的图像平均灰度值都小于一个设定阈值P_Th的所有图像子块，将符合条件的所有子块区域合并为一个区域。如果该区域为非空，那么计算该区域的重心坐标，若该坐标在中心子块区域内，则继续执行步骤三；若不在中心子块区域内则继续执行步骤二对胶囊机器人的姿态进行调整。

如果计算每个子块的RGB通道图像的平均灰度都不满足小于一个设定阈值P_Th的条件，那么此时控制胶囊机器人分别以偏航角±δ₀和俯仰角±δ₀向四个方向进行旋转，在完成旋转控制后分别采集图像，再次对四幅图像进行分块处理并计算每个子块的RGB通道图像的平均灰度，找到平均灰度最小的子块，继续执行步骤二对胶囊机器人的姿态进行调整；若该子块的平均灰度值大于阈值P_Th，则继续执行上述处理过程，直至找到子块的平均灰度值小于阈值P_Th结束。

步骤二，根据上一步得到的方向信息对胶囊机器人的姿态进行调整。

本步骤的主要执行过程如下：如果上一步计算得到的目标区域为非空，那么控制胶囊机器人向当前图像中灰度均值最小的子块区域I₀的重心P移动；为了表示胶囊机器人的空间位姿，建立全局坐标系OXYZ和局部相机坐标系oxyz，假设起始时刻全局坐标系OZ轴与局部坐标系oz轴重合，现需要调整oz轴指向OP的方向，如图一所示。图中图像坐标系为O_iX_iY_i，原点O为相机光轴与图像中心的交点。

假设子块区域I₀的重心P在图像中的坐标为(x_i，y_i)，在局部相机坐标系下的坐标为(x_c，y_c，z_c)，则根据相机成像关系可得的单位方向矢量为

其中，f为相机焦距。

通过***的定位模块计算出起始时刻相机在全局坐标系下的位姿，得到胶囊机器人在相机坐标系下绕oy轴的初始旋转角度(偏航角)α₀和绕ox轴的初始旋转角度(俯仰角)β₀；利用旋转矩阵变换，将的单位方向矢量从相机坐标系变换到世界坐标系下：

而在世界坐标系下又可以表示为下式，其中α为世界坐标系下的偏航角，β为世界坐标系下的俯仰角。

则根据式(2)和(3)可得α和β分别为：

通过上述计算过程可以得到的α和β即为修正胶囊机器人前进方向的调整偏航角度和俯仰角度。这样目标区域的重心坐标落在图像的中心子块中。

步骤三，对肠道孔腔的图像进行提取褶皱轮廓边缘信息，根据相机的成像模型将二维的褶皱轮廓反向映射到三维空间。

本步骤的主要执行过程如下：对肠道孔腔的图像进行预处理滤除噪声，并采用Canny算法进行边缘检测提取出非连续的轮廓线段；通过形态学操作去除掉噪声引起的假边缘，得到候选轮廓集合A，保留的轮廓线集合A中属于同一褶皱的轮廓做平滑连接，判断属于同一褶皱的标准如下：集合A中找到端点间距离最小的两段轮廓线，然后计算两端点分别与孔腔中心点连接所形成的夹角、两端点连线分别与两段轮廓线所选端点的切线形成的夹角，如果第一个夹角角度小于30度，并且第二、三个夹角角度同时小于45度，那么就认为这两段轮廓线属于同一褶皱，将这两个端点进行平滑连接形成一段新轮廓，并把新轮廓放回到集合A中继续执行此过程，直到集合A中没有满足标准的轮廓线为止。对集合A中的非闭合轮廓，按照此轮廓线内侧邻近一层的闭合轮廓的对应位置上的轮廓形状进行等比例拟合处理。

利用相机标定得到内参矩阵可以建立起相机的映射模型，通过映射模型把得到的二维闭合褶皱轮廓上的像素点沿着通过光心的光线映射到三维空间上。如图2所示，通过反向映射可以把像素点u转化成三维空间向量根据Lambertian反射定理可以计算出像素点的深度信息d_u，根据公式一得到该点的三维信息。

其中，(x(u)，y(u))分别为像素点u的二维坐标。δ_u为反向映射向量与Z轴夹角，r_u为点u到图像中心的距离。

步骤四，在胶囊机器人重建出局部三维结构后，利用定位模块给出全局位姿。

本步骤的主要执行过程如下：在胶囊机器人调整自身姿态，使得前端相机对准肠道孔腔方向，并利用采集图像初次重建出局部三维肠道结构模型，通过***的定位模块计算出相机当前的全局坐标系下的位姿矩阵M_G；而此时相机位置定义为重建肠道模型局部坐标系的原点，相机光轴方向为局部坐标系的Y轴正向，局部坐标系下相机位姿矩阵为M_L；控制胶囊机器人沿着重建局部模型的前向相邻两个褶皱轮廓的中心点连线方向前进，期间相机的光轴与肠道弯曲方向相一致，则从起始帧开始利用每帧包含孔腔的图像重建出局部三维模型，根据光流法对轮廓信息进行追踪计算出运动矢量，用运动信息找出两帧图像中共有的轮廓，并用共有轮廓的三维点集按照对应关系分别放入集合(Q_x，Q_y，Q_z)和(P_x，P_y，P_z)，然后通过求解公式六所示最小二乘方程得到运动矩阵Mat3D。

为了提高精度，采用连续多帧配准方法找到具有最小均方误差的结果为最优配准模型，得到相机的运动矩阵M_i；则轮廓上的三维点集在世界坐标系下的坐标P_w为：

此时，通过***的定位模块计算出在世界坐标系下的相机位姿M_G′，利用世界坐标系下重建局部模型中相机所在位置p_G′，找到距离最近的前后两个闭合褶皱轮廓的中心点p_c1和p_c2，连接两个中心点形成在三维空间中的前进轨迹矢量计算出相机到直线的距离d和方向控制胶囊机器人沿着方向移动距离d，使得胶囊机器人回到规划的运动轨迹上；然后计算相机朝向方向与前进轨迹矢量间的夹角θ，即两个局部坐标系的Y轴之间的夹角，如公式八所示；通过***控制单元调整相机朝向与前进轨迹矢量平行，这样就在相机视野里找到了肠道孔腔的弯曲方向，具体如图三所示。

Claims (10)

1.一种基于单目图像的胶囊机器人交互控制方法，其特征在于，通过胶囊机器人前端的单目相机采集胃肠图像，对所述胃肠图像进行处理，提取胃肠结构的褶皱轮廓信息，重建出胃肠局部三维结构；将所述胃肠局部三维结构结合相机位姿信息来调整胶囊机器人的前进方向。

2.如权利要求1所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，对胶囊机器人采集的胃肠图像进行分块处理，对每个子图像块进行滤波和特征提取。

3.如权利要求2所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，胃肠图像的分辨率为M*N，所述胃肠图像进行分块采用以下方式，图像的行和列方向上分别都分成n段，其中n为奇数，形成n*n个分辨率为M/n*N/n的图像子块。

4.如权利要求1所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，所述单目相机采集胃肠图像后，通过图像平均灰度信息判断是否找到肠道孔腔，如果图像中包括孔腔信息则利用相机内参数构造成像模型，得到孔腔方向的三维向量并计算调整胶囊机器人的偏航角和俯仰角，使得孔腔区域落在图像中间区域。

5.如权利要求1所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，所述单目相机采集胃肠图像后，如果图像中不包括孔腔信息则控制胶囊机器人分别以偏航角和俯仰角向四个方向进行旋转，找到平均灰度最小的子块为目标区域，利用相机内参数构造成像模型，得到目标区域的三维向量并计算调整胶囊机器人的偏航角和俯仰角，使得目标区域落在图像中间区域，重复此过程直至在图像中找到孔腔区域。

6.如权利要求4或5所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，所述重建出胃肠局部三维结构采用以下方式：根据所述成像模型构造反向映射模型，根据反向映射模型将采集到的胃肠图像中二维褶皱轮廓信息转换到三维空间，并根据Lambertian反射定理计算出深度信息，重建出胃肠局部的三维结构。

7.如权利要求6所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，所述二维褶皱轮廓信息采用以下方式获得：对肠道孔腔的图像进行预处理滤除噪声，并采用Canny算法进行边缘检测提取出非连续的轮廓线段；通过形态学操作去除掉噪声引起的假边缘，得到候选轮廓集合A，保留轮廓线集合中属于同一褶皱的轮廓并做平滑连接。

8.如权利要求7所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，判断所述集合A属于同一褶皱的方法如下：在轮廓线集合A中找到端点间距离最小的两段轮廓线，然后计算两端点分别与孔腔中心点连接所形成的夹角、两端点连线分别与两段轮廓线所选端点的切线形成的夹角，如果第一个夹角角度小于θ₀度，并且第二、三个夹角角度同时小于θ₁度，那么就认为这两段轮廓线属于同一褶皱。

9.如权利要求7或8所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，以相邻褶皱间的中心连线作为规划路径，计算出胶囊机器人的调整方向和距离。

10.如权利要求9所述的一种基于单目图像的胶囊机器人交互控制方法，其特征在于，通过定位模块计算出在世界坐标系下的相机位姿，利用世界坐标系下重建局部模型中相机所在位置，找到距离最近的前后两个闭合褶皱轮廓的中心点，连接两个中心点形成在三维空间中的前进轨迹矢量，计算出相机到直线的距离d和方向n，控制胶囊机器人沿着方向n移动距离d，使得胶囊机器人回到规划的运动轨迹上；然后计算相机朝向方向与前进轨迹矢量间的夹角θ，即两个局部坐标系的Y轴之间的夹角，通过***控制单元调整相机朝向与前进轨迹矢量平行。