CN108759812A - 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 - Google Patents

Abstract

本发明属于自动化工程技术领域，涉及三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面，运用图像处理，对获取的图像进行目标特征点的识别与提取，并根据单目视觉测距原理将目标区域到摄像机的距离转化为摄像机光心到具体特征点的距离。通过改变磁矢量轴线方位带动胶囊摄像机原地进行待测区域特征目标的搜索，根据摄像机图像确定胶囊机器人和待测目标相对于参考位置的空间距离，结合胶囊机器人两次获得目标特征时的俯仰角和側摆角信息，以及胶囊机器人与待测目标相对参考位置的空间几何关系，确定胶囊机器人与待测目标相对待测区域某参考位置的定位方法。

Description

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

技术领域

本发明属于自动化工程技术领域，涉及三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。

背景技术

移动机器人的定位问题，特别是在未知、复杂、动态的非结构环境中(如人体胃肠道)的定位问题是当前移动机器人研究领域亟待解决的关键问题。

现有自主移动机器人的定位技术可分为以下几种：1.基于路标的定位技术。路标是指具有明显特征的，能够被移动机器人传感器识别的特殊物体，路标在全局环境中具有固定和已知的位置，移动机器人定位的主要任务就是可靠辨识路标，并计算出移动机器人的位置，其定位精度的高低取决于对路标的识别以及位置信息的准确提取程度。然而在封闭狭窄非结构环境中，其环境信息通常都是未知的，通过人为设置具体的路标来更不现实，因此这种方法无法适应三维封闭非结构空间内移动机器人的定位需求。2.基于概率方法的移动机器人定位技术。该方法的理论基础是贝叶斯滤波BF(Bayesian Filter)概率定位方法，其核心思想为用传感器测量数据去估计一个动态环境的未知状态，并以当前为止收集到的数据为条件，递归估计状态空间后验概率密度，并可具体分为卡尔曼滤波方法、马尔可夫方法和多假设跟踪MHL方法。卡尔曼滤波算法是一种高效、高精度的局部定位方法，其约束条件太多，只能表示单值的概率分布，无法解决全局定位和机器人的诱拐问题。马尔可夫方法虽然适合全局定位，但是需处理大量的概率数值，且定位精度不高，而多假设跟踪MHL方法由于依赖于卡尔曼滤波的线性假设，其实际实现性较差。3.基于地图的定位技术。该定位方法的核心在于通过机器人感知获取的局部环境信息与已知地图中的环境信息进行匹配，从而实现机器人自身位置的辨别。由于该定位方法需建立工作环境的地图信息，而在未知、复杂、非结构的动态环境中，通常无法提前构建移动机器人定位所需的地图信息。

为实现三维封闭非结构动态环境中胶囊机器人的定位，我们课题组在已有国家发明专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利申请号：201510262778.4)和“一种空间万向旋转磁场人机交互控制方法”(专利号：ZL201610009285.4)的基础上，结合双半球胶囊机器人定点悬停调姿功能和人机交互控制方法，提出磁场与视觉共融的胶囊机器人定位法。

具体以磁驱双半球胶囊机器人在悬停调姿下的定点全景观察功能为手段，借助磁场与视觉共融的人机交互功能，将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面，运用图像处理对获取的图像进行目标特征点的识别与提取，并根据单目视觉测距原理将目标区域到摄像机的距离转化为摄像机光心到具体特征点的距离。通过改变磁矢量轴线方位带动胶囊摄像机原地进行待测区域特征目标的搜索，并选定某一特征目标作为位置参考，根据摄像机图像确定胶囊机器人和待测目标相对于参考位置的空间距离，之后结合胶囊机器人两次获得目标特征时的俯仰角和側摆角信息，以及胶囊机器人与待测目标相对参考位置的空间几何关系，确定胶囊机器人与待测区域方位信息。

目前，尚未有人提出基于磁场与视觉共融的胶囊机器人及待测目标的相对定位方法，该定位方法的突出特点是无需构建移动机器人传统定位中所需的人工路标和地图，也不需要增加额外其他检测测量装置，仅依靠胶囊机器人的摄像机视觉并结合其自身的定点悬停调姿功能便可获取用于胶囊和待测目标定位的各种关键信息。

发明内容

本发明要解决的技术方案是提供一种三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。具体以磁驱双半球胶囊机器人在悬停调姿下的定点全景观察功能为手段，借助磁场与视觉共融的人机交互功能，将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面，运用图像分割算法、粒子群加速算法以及纹理与暗区识别理论，对获取的图像进行目标特征点的识别和提取，并根据单目视觉空间测距模型将目标物到摄像机的距离转化为摄像机光心到具体特征点的距离，从而确定出封闭三维空间内待测目标和胶囊机器人相对于某参考目标的空间距离，之后结合胶囊机器人两次获得目标特征时的俯仰側摆角以及参考目标、胶囊机器人和待测目标三者之间的空间几何关系便可确定出胶囊机器人与待测目标的方位。

本发明的技术方案是：

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

第一步，首先根据小孔成像模型以及摄像机成像点与目标点的映射关系，建立以世界坐标表示的点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各坐标轴的夹角相同；之后根据胶囊机器人和待测目标与位置参考点距离及空间位置关系，并结合两次观测到目标区域时的俯仰、侧摆角信息，可分别确定出待测目标和胶囊机器人相对于位置参考点的空间方位信息。

上述第一步中，根据小孔成像模型建立单目视觉空间测距模型的推导过程如下：如图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为原点建立二维平面坐标系O_wX_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₀O P_y'中，满足

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

P_xO_w＝O₀O_w tanγ (4)

在三角形O₀O₁O_w中

在三角形O₀OP_x'中

由式(2)(3)可得

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

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

如图2所示，双半球胶囊机器人的摄像机安装在机器人的顶部，胶囊机器人的半径为R，胶囊轴线在竖直面内的俯仰角为δ，并定义胶囊机器人定点扫描观察时，侧摆和俯仰角逆时针为正，根据图2中的几何关系，可得摄像机光心到水平面的高度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₀为摄像机内部参数，可由摄像机定标确定。

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

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

以参考标志形心为坐标原点，建立如图3b所示的世界坐标系O_wX_wY_wZ_w。图3b中，位置参考点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δ₂)。

本发明的效果和益处是：

该定位方法以磁驱双半球胶囊机器人悬停调姿下的定点全景观察功能为手段，借助磁场与视觉共融的人机交互功能，通过磁矢量方位感知胶囊轴线方位，不仅实现了胶囊机器人在三维非结构空间内实时方位的确定，还实现了该环境中任意待测目标的方位确定。待测目标方位的确定为胶囊机器人针对待测目标的进一步活动提供了必要的技术支撑，胶囊机器人实时位置的确定则为构建完整的主动反馈控制***奠定了坚实的基础。该种定位方法还解决了影响视觉定位精度的关键难题(诸如摄像机俯仰角和光心高度的实时测量问题)，提高了视觉定位的准确性和实时性。其显著特点是无需构建移动机器人传统定位中所需的人工路标和地图，也无需增加其他额外定位检测装置，仅依靠胶囊机器人自身的机器视觉和其定点全景观察功能便可实现其自身方位和任意待测目标方位的确定，该种定位方法具有安全性高，实用性好，环境适应能力强、工作环境要求低，经济可靠的优点。

附图说明

图1是单目视觉空间测距示意图。

图2是胶囊摄像机光心到水平面的高度确定示意图。

图3(a)是胶囊机器人在胃下部区域以幽门作参考点时定位示意图。

图3(b)是以胃幽门作参考点时胶囊机器人与待测目标相对方位确定示意图。

图4(a)胶囊机器人人机交互控制的技术方案示意图。

图4(b)是主被动双半球形胶囊机器人外部结构局部放大图。

图4(c)是主被动双半球形胶囊机器人内部结构局部放大图。

图5(a)是胶囊机器人在胃下部区域以幽门作参考点时定位示意图。

图5(b)是以胃幽门作参考点时胶囊机器人与待测目标相对方位确定示意图。

图中：a患者；b病床；c三轴正交嵌套亥姆霍兹线圈磁场叠加装置；d主被动双半球形胶囊机器人；e₁侧摆操纵杆；e₂俯仰操纵杆；f信号处理器；g外磁场驱动器；h人机交互界面；n_B调姿磁场旋转轴线；

1主动半球壳；2被动半球壳；3轴承定位套筒；4轴承；5圆螺母；6阶梯轴；7径向磁化钕铁硼圆环内驱动器；8摄像头与图像传输装置。

具体实施方式

以下以胃内三维非结构封闭空间的胶囊机器人定位为例，结合技术方案和附图详细叙述具体实施例。

如图4a所示，将三组线圈相互正交嵌套安装成三轴正交嵌套亥姆霍兹线圈磁场叠加装置c，让患者a吞下主被动双半球形胶囊机器人d，并躺在病床b上，调整病床b的位置使患者d的胃部处于三轴正交嵌套亥姆霍兹线圈磁场叠加装置c的中心区域。

实施例1：

当胶囊机器人在胃下部区域时，可按以下步骤实施胶囊机器人和胃内待测目标的定位：

(1)胶囊机器人进入待测区域后(如图3a所示)，通过侧摆操纵杆e₁与俯仰操纵杆e₂使磁矢量轴线侧摆角θ和俯仰角δ分别在0～360°和0～90°内变化，旋转磁矢量与胶囊机器人内嵌径向磁化钕铁硼圆环驱动器相互作用，在磁力矩随动效应下，胶囊机器人轴线始终追随旋转磁矢量轴线改变方位并与其保持一致，从而带动胶囊摄像机在待测区域实现基于定点悬停调姿的全景观察功能。摄像机将拍摄到的实时图像通过无线传输到人机交互界面。当通过扫描观测到胃部幽门管时，根据摄像机拍摄的图像运用基于纹理和暗区相融合的离子群加速算法识别胃幽门管的形心O_w，运用单目视觉测距模型计算摄像机光心到幽门管形心的距离L₁，并根据式(14)得到机器人球心P₁与幽门管形心O_w的空间距离d₁，同时从磁场控制器读取此时胶囊机器人的侧摆角θ₁和俯仰角δ₁信息。

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

(3)以胃幽门管形心O_w为原点，建立与外部磁场坐标系OXYZ相平行的世界坐标系O_wX_wY_wZ_w，根据胶囊机器人和待测目标与参考点的距离及空间位置关系，并结合两次观测到目标区域时的俯仰、侧摆角信息，可分别确定出胶囊机器人和待测目标相对于幽门管形心的方位信息。

具体定位过程为：如图3b所示，根据胶囊机器人和待测目标与参考点的空间位置关系以及两次观测到目标图像时的俯仰、侧摆角信息，将胶囊机器人到原点O_w的距离d₁和到待测目标的距离d₂分别向世界坐标系O_wX_wY_wZ_w的三个坐标轴上投影，由式(17)(18)可分别求得胶囊机器人和待测目标相对胃幽门的相对方位。

实施例2：

当胶囊机器人处于到胃上部区域时，参考实施例1可按以下步骤进行进行定位作业：

(1)胶囊机器人运动到胃上部区域后(如图5a所示)，通过侧摆操纵杆e₁与俯仰操纵杆e₂使磁矢量轴线侧摆角θ和俯仰角δ分别在0～360°和0～90°内变化，旋转磁矢量与胶囊机器人内嵌径向磁化钕铁硼圆环驱动器相互作用，在磁力矩随动效应下，胶囊机器人轴线始终追随旋转磁矢量轴线改变方位并与其保持一致，从而带动胶囊摄像机在胃部实现基于定点悬停调姿的全景观察功能。摄像机将拍摄到时实时图像通过无线传输到人机交互界面。当通过扫描观测到胃部贲门管时，根据图像处理识别胃贲门管的形心，运用单目视觉测距模型，计算摄像机光心到贲门管形心的距离L₁，并根据式(14)得到机器人球心P₁与贲门管形心O_w的空间距离d₁，同时从磁场控制器读取此时胶囊机器人的侧摆角θ₁和俯仰角δ₁信息。

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

(3)以胃贲门管形心为原点，建立与外部磁场坐标系OXYZ相平行的世界坐标系O_wX_wY_wZ_w，根据机器人球心与贲门管形心和待测目标形心的距离d₁、d₂及空间位置关系，并结合两次观测到目标区域时的俯仰、侧摆角信息，可分别确定出胶囊机器人和待测目标相对于胃贲门的方位信息。

具体计算过程为：如图5b所示，根据胶囊机器人和待测目标与胃贲门的空间位置关系以及两次观测到目标图像时的俯仰、侧摆角信息，将胶囊机器人到原点O_w的距离d₁和到待测目标的距离d₂分别向世界坐标系O_wX_wY_wZ_w的三个坐标轴上投影，由式(17)(18)可分别求得胶囊机器人和待测目标相对胃贲门的方位信息。

Claims (3)

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各坐标轴的夹角，两个夹角相同；之后根据胶囊机器人和待测目标与位置参考点距离及空间位置关系，并结合两次观测到目标区域时的俯仰、侧摆角信息，分别确定出待测目标和胶囊机器人相对于位置参考点的空间方位信息。

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₀O P_y'中，满足

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

P_xO_w＝O₀O_w tanγ (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₀为摄像机内部参数，由摄像机定标确定。

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

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

以参考标志形心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δ₁)。