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

权利要求书

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

第一步：当双半球型胶囊机器人在当前位置上需要进行导航时，通过铅垂角度传感器，确定摄像头框架绕双半球型胶囊机器人轴线的自转角度；

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

第三步：运用图像处理，提取拐弯处的暗区质心像素坐标，以此作为肠道转弯导航点；双半球型胶囊机器人始终处于三轴亥姆霍兹线圈磁场均匀区域，通过坐标变换，将图像暗区质心的像素坐标转化为在固定坐标系内质心相对于双半球型胶囊机器人的方向向量，再由双半球型胶囊机器人运动原理所要求的施加滚动磁场磁矢量方向需与水平面平行，计算出从当前位置运动到目标位置需要施加的滚动磁场磁矢量方向，实现双半球型胶囊机器人的导航；其过程具体为：当双半球型胶囊机器人进入工作环境时，需获知其在体内的位置及姿态才能进行导航；在磁场均匀区内，双半球型胶囊机器人的轴线方向始终与磁矢量方向一致，因此将固定坐标系O-XYZ的原点O平移至摄像头的光心O_C，坐标轴方向不变，构成动坐标系O_C-X_wY_wZ_w，摄像头相对于固定坐标系的姿态等价于摄像头相对于动坐标系的姿态，且所要求解的在固定坐标系内暗区质心相对于双半球型胶囊机器人的方向向量等价于暗区质心在动坐标系O_C-X_wY_wZ_w内的方向向量；因此只需要计算出目标点在动坐标系中的方向向量，此向量即最终导航方向；进而，只需要得到双半球型胶囊机器人的姿态信息，而不需要获知双半球型胶囊机器人在固定坐标系内的具体位置信息；设P点是暗区质心在动坐标系O_C-X_wY_wZ_w中的位置，p为P点在像平面上的成像点，动坐标系内向量O_C-P的方向信息与向量O_C-p的方向一致，所以求动坐标系内向量O_C-P的方向信息能转化为求动坐标系内向量O_C-p的方向信息；

建立坐标系：首先，在已建立的动坐标系O_C-X_WY_WZ_W的基础上，建立双半球型胶囊机器人轴线坐标系O_C-X₁Y₁Z₁，O_C-X₁轴在水平方向上，O_C-Z₁轴与双半球型胶囊机器人轴线方向重合；建立摄像头轴线坐标系O_C-X_CY_CZ_C，坐标系O_C-X_CY_CZ_C由坐标系O_C-X₁Y₁Z₁绕O_C-Z₁轴顺时针旋转α角度得到，O_C-Z_C轴与O_C-Z₁轴重合；建立图像物理坐标系O₁-xy，O₁O_C的长度为摄像头焦距，O₁-xy垂直于O_C-Z_C轴，且O₁-x轴与O_C-X_C轴平行，O₁-y轴与O_C-Y_C轴平行；建立计算机图像坐标系O₀-uv，计算机图像坐标系与图像物理坐标系之间关系为：

经过转换得到：

求逆后得到图像物理坐标系和计算机图像坐标系之间的关系：

其中，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为摄像头焦距；

在摄像头轴线坐标系O_C-X_CY_CZ_C中，像平面上p点的Z_C坐标值为摄像头的焦距f，因此p点在摄像头轴线坐标系O_c-X_cY_cZ_c中的坐标为(x,y,f)；

双半球型胶囊机器人轴线坐标系O_C-X₁Y₁Z₁和摄像头轴线坐标系Oc-XcYcZc间转换：摄像头坐标系O_C-X_CY_CZ_C是由双半球型胶囊机器人轴线坐标系O_C-X₁Y₁Z₁绕O_C-Z₁轴顺时针旋转α角度得到，则得公式(5)：

逆运算得：

动坐标系O_C-X_WY_WZ_W和双半球型胶囊机器人轴线坐标系O_C-X₁Y₁Z₁的转换过程为：初始在X_WO_CZ_W平面内旋转的磁矢量B，其旋转平面法向量n与O_C-Y_W轴重合，为了保证变换过程中X轴始终在水平面内，首先绕O_C-Z_W轴顺时针旋转θ角，再绕O_C-X₁轴逆时针旋转δ角，即得到坐标系O_C-X₁Y₁Z₁，其中θ角为侧摆角，δ角为俯仰角；经旋转后，磁矢量平面法向量由O_C-Y_W轴变为n＝(sinθcosδ cosθcosδ sinδ)，正交变换矩阵为

则

得到

其中

所以，结合以上坐标变换，当双半球型胶囊机器人在旋转磁场作用下，磁矢量方向侧摆角为θ，俯仰角为δ，摄像头绕双半球型胶囊机器人轴线的自转角度为α，图像上暗区质心的计算机图像坐标为(u,v)，由以上参数计算图像暗区质心在动坐标系O_C-X_WY_WZ_W内的方向n_S：

其中

再通过正交变换方法，将n_S(X_W,Y_W,Z_W)变换为用侧摆角θ和俯仰角δ两个变量表示，即得到了最终导航方向。