[发明专利]三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法

权利要求书

1.三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法，其特征在于，步骤如下：

第一步，首先根据小孔成像模型以及摄像机成像点与目标点的映射关系，建立以世界坐标表示的点P(X_W,Y_W,Z_W)与以像素为单位的图像坐标P″(u,v)之间的关系；之后通过对目标图像的处理和分析，提取目标图像特征点，将目标物到摄像机的距离关系转化为具体特征点与摄像机光心之间距离关系，最终建立基于单目视觉的空间测距模型；

第二步，胶囊机器人进入三维非结构封闭空间后，依据被动模态定点悬停调姿，通过控制旋转磁矢量轴线的侧摆角和俯仰角改变其方位，在磁力矩随动效应下，胶囊机器人轴线与旋转磁场轴线始终保持一致，并带动胶囊机器人摄像机在待测区域定点侧摆俯仰扫描观察；当观测到某参考标志时，对获取的图像进行图像处理，提取该参考标志的形心并将其作为位置参考点；之后运用单目视觉空间测距模型，计算摄像机光心到位置参考点的距离L₁，并从磁场控制器读取摄像机获得参考位置图像时胶囊机器人的侧摆角θ₁和俯仰角δ₁；

第三步，通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索，当检测到待测目标后再次通过图像处理识别待测目标形心，并根据上述测距方式计算待测目标形心到摄像机光心的距离L₂，同时从磁场控制器读取此时胶囊机器人的侧摆角θ₂和俯仰角δ₂信息；

第四步，以第二步确定的位置参考点为原点，建立与外部磁场坐标系OXYZ相平行的世界坐标系O_wX_wY_wZ_w，由于该坐标系由外部磁场坐标系OXYZ经空间平移得到，且胶囊机器人处于外部磁场的均匀区内，胶囊机器人轴线与磁场坐标系OXYZ各坐标轴的夹角、胶囊机器人轴线与世界坐标系O_wX_wY_wZ_w各坐标轴的夹角，两个夹角相同；之后根据胶囊机器人和待测目标与位置参考点距离及空间位置关系，并结合两次观测到目标区域时的俯仰、侧摆角信息，分别确定出待测目标和胶囊机器人相对于位置参考点的空间方位信息；

所述的第二、三步中，待测目标及胶囊机器人的方位确定过程为：胶囊机器人在待测区域依据被动模态定点悬停调姿功能，通过控制旋转磁矢量的侧摆角和俯仰角带动胶囊机器人摄像机在P₁定点扫描观察，运用图像处理识别某参考标志的形心，并将其作为确定胶囊机器人和待测目标方位的参考点；从磁场控制器读取摄像机获得参考标志图像时胶囊机器人的侧摆角θ₁和俯仰角δ₁，设该参考标志的形心图像坐标为P₁'(u₁，v₁)，根据单目视觉测距原理由式(14)得胶囊机器人球心到该参考标志的形心的距离d₁为

通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索，当检测到待测目标后再次通过图像处理识别其形心P₂，并通过磁场控制器读取此时胶囊机器人的侧摆角θ₂和俯仰角δ₂；设待测目标形心的图像坐标为P₂'(u₂，v₂)，根据上述方法计算得胶囊机器人球心到待测目标形心的距离d₂为

其中，a_x，a_y，u₀和v₀为摄像机内部参数，可由摄像机定标确定；

以位置参考点O_w为坐标原点，建立世界坐标系O_wX_wY_wZ_w；位置参考点O_w和待测目标形心P₂在点P₁所在的水平面内的投影分别为O₂、P₂”,设P₁点坐标为(X₁，Y₁，Z₁)，则由图中几何关系并结合式(15)(16)得

设P₂点坐标为(X₂，Y₂，Z₂)，则

综上，胶囊机器人在世界坐标系O_wX_wY_wZ_w中的实时方位表示为：

P₁(d₁cosδ₁sinθ₁,-d₁cosδ₁cosθ₁,-d₁sinδ₁)

待测目标的方位表示为：

P₂(d₁cosδ₁sinθ₁-d₂cosδ₂sinθ₂,-d₁cosδ₁cosθ₁+d₂cosδ₂cosθ₂,d₂sinδ₂-d₁sinδ₁)。

2.根据权利要求1所述的三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法，其特征在于，所述的第一步中，根据小孔成像模型建立单目视觉空间测距模型的推导过程如下：

O₀为摄像机的光心，O(x₀,y₀)为摄像机光轴O₀O_w与像平面的交点，以O(x₀,y₀)为原点建立像平面坐标系Oxy，同时以成像面右上角为坐标原点建立与Oxy各坐标轴平行的以像素为单位的坐标系ouv；设摄像机在竖直面内俯仰δ角观测到平面上一目标点P，P′为被测点P在像平面上的映射点，P_x'、P_y'点分别为P'在x、y轴上的投影点；以光轴与水平面的交点O_w为原点建立二维平面坐标系OX_wY_w,点P在各坐标轴上的投影点分别为P_x、P_y；摄像机光心O₀在水平面的投影点为O₁，距离水平面的高度为h，摄像机的焦距为f，即OO₀＝f；

设光轴O₀O与O₀P_y'的夹角为β，与O₀ P_x'的夹角为γ，P'点在坐标系Oxy中的坐标为P'(x₁,y₁)，摄像机光心O₀与点P的空间距离为L，则

在三角形O₀PP_y中，L表示为

在三角形O₀O₁P_y中，O₀P_y表示为

在三角形O₀OP_y'中，满足

在坐标系O_wX_wY_w中，由投影关系知PP_y＝P_xO_w，在三角形O₀O_wP_x中

P_xO_w＝O₀O_wtanγ (4)

在三角形O₀O₁O_w中

在三角形O₀OP_x'中

由式(2)(3)得

由式(4)(5)(6)得

由式(1)(7)(8)得

双半球胶囊机器人的摄像机安装在机器人的顶部，胶囊机器人的半径为R，胶囊轴线在竖直面内的俯仰角为δ，并定义胶囊机器人定点扫描观察时，侧摆和俯仰角逆时针为正，根据几何关系，得摄像机光心到水平面的高度h为

h＝R(1+sinδ) (10)

由式(9)(10)得

如果(u,v)为以像素为单位的图像坐标系中的坐标，则O″(u₀,v₀)为摄像机光轴与像平面交点O(x₀,y₀)的以像素为单位的坐标，P″(u,v)是P′(x,y)的坐标，设像平面上一个像素在x轴和y轴上的物理尺寸分别为dx、dy，则有：

由式(12)得x＝(u-u₀)dx，y＝(v-v₀)dy，令a_x＝f/dx,a_y＝f/dy，由式(11)得被测点P与摄像机光心之间的空间距离为

双半球胶囊机器人球心到摄像机光心的距离近似等于球形机器人半径R，因此得机器人球心到被测点的空间距离d表示为

其中，a_x，a_y，u₀和v₀为摄像机内部参数，由摄像机定标确定。