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

权利要求书

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变换为用θ和δ两个变量表示，即得到了最终导航方向。