CN108759812B - 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 - Google Patents
三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 Download PDFInfo
- Publication number
- CN108759812B CN108759812B CN201810285619.XA CN201810285619A CN108759812B CN 108759812 B CN108759812 B CN 108759812B CN 201810285619 A CN201810285619 A CN 201810285619A CN 108759812 B CN108759812 B CN 108759812B
- Authority
- CN
- China
- Prior art keywords
- target
- camera
- capsule robot
- point
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- Multimedia (AREA)
- Manipulator (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明属于自动化工程技术领域,涉及三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面,运用图像处理,对获取的图像进行目标特征点的识别与提取,并根据单目视觉测距原理将目标区域到摄像机的距离转化为摄像机光心到具体特征点的距离。通过改变磁矢量轴线方位带动胶囊摄像机原地进行待测区域特征目标的搜索,根据摄像机图像确定胶囊机器人和待测目标相对于参考位置的空间距离,结合胶囊机器人两次获得目标特征时的俯仰角和側摆角信息,以及胶囊机器人与待测目标相对参考位置的空间几何关系,确定胶囊机器人与待测目标相对待测区域某参考位置的定位方法。
Description
技术领域
本发明属于自动化工程技术领域,涉及三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。
背景技术
移动机器人的定位问题,特别是在未知、复杂、动态的非结构环境中(如人体胃肠道)的定位问题是当前移动机器人研究领域亟待解决的关键问题。
现有自主移动机器人的定位技术可分为以下几种:1.基于路标的定位技术。路标是指具有明显特征的,能够被移动机器人传感器识别的特殊物体,路标在全局环境中具有固定和已知的位置,移动机器人定位的主要任务就是可靠辨识路标,并计算出移动机器人的位置,其定位精度的高低取决于对路标的识别以及位置信息的准确提取程度。然而在封闭狭窄非结构环境中,其环境信息通常都是未知的,通过人为设置具体的路标来更不现实,因此这种方法无法适应三维封闭非结构空间内移动机器人的定位需求。2.基于概率方法的移动机器人定位技术。该方法的理论基础是贝叶斯滤波BF(Bayesian Filter)概率定位方法,其核心思想为用传感器测量数据去估计一个动态环境的未知状态,并以当前为止收集到的数据为条件,递归估计状态空间后验概率密度,并可具体分为卡尔曼滤波方法、马尔可夫方法和多假设跟踪MHL方法。卡尔曼滤波算法是一种高效、高精度的局部定位方法,其约束条件太多,只能表示单值的概率分布,无法解决全局定位和机器人的诱拐问题。马尔可夫方法虽然适合全局定位,但是需处理大量的概率数值,且定位精度不高,而多假设跟踪MHL方法由于依赖于卡尔曼滤波的线性假设,其实际实现性较差。3.基于地图的定位技术。该定位方法的核心在于通过机器人感知获取的局部环境信息与已知地图中的环境信息进行匹配,从而实现机器人自身位置的辨别。由于该定位方法需建立工作环境的地图信息,而在未知、复杂、非结构的动态环境中,通常无法提前构建移动机器人定位所需的地图信息。
为实现三维封闭非结构动态环境中胶囊机器人的定位,我们课题组在已有国家发明专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利申请号:201510262778.4)和“一种空间万向旋转磁场人机交互控制方法”(专利号:ZL201610009285.4)的基础上,结合双半球胶囊机器人定点悬停调姿功能和人机交互控制方法,提出磁场与视觉共融的胶囊机器人定位法。
具体以磁驱双半球胶囊机器人在悬停调姿下的定点全景观察功能为手段,借助磁场与视觉共融的人机交互功能,将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面,运用图像处理对获取的图像进行目标特征点的识别与提取,并根据单目视觉测距原理将目标区域到摄像机的距离转化为摄像机光心到具体特征点的距离。通过改变磁矢量轴线方位带动胶囊摄像机原地进行待测区域特征目标的搜索,并选定某一特征目标作为位置参考,根据摄像机图像确定胶囊机器人和待测目标相对于参考位置的空间距离,之后结合胶囊机器人两次获得目标特征时的俯仰角和側摆角信息,以及胶囊机器人与待测目标相对参考位置的空间几何关系,确定胶囊机器人与待测区域方位信息。
目前,尚未有人提出基于磁场与视觉共融的胶囊机器人及待测目标的相对定位方法,该定位方法的突出特点是无需构建移动机器人传统定位中所需的人工路标和地图,也不需要增加额外其他检测测量装置,仅依靠胶囊机器人的摄像机视觉并结合其自身的定点悬停调姿功能便可获取用于胶囊和待测目标定位的各种关键信息。
发明内容
本发明要解决的技术方案是提供一种三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法。具体以磁驱双半球胶囊机器人在悬停调姿下的定点全景观察功能为手段,借助磁场与视觉共融的人机交互功能,将胶囊机器人定点悬停调姿时所获得的实时图像通过无线传输至人机交互界面,运用图像分割算法、粒子群加速算法以及纹理与暗区识别理论,对获取的图像进行目标特征点的识别和提取,并根据单目视觉空间测距模型将目标物到摄像机的距离转化为摄像机光心到具体特征点的距离,从而确定出封闭三维空间内待测目标和胶囊机器人相对于某参考目标的空间距离,之后结合胶囊机器人两次获得目标特征时的俯仰側摆角以及参考目标、胶囊机器人和待测目标三者之间的空间几何关系便可确定出胶囊机器人与待测目标的方位。
本发明的技术方案是:
三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法,步骤如下:
第一步,首先根据小孔成像模型以及摄像机成像点与目标点的映射关系,建立以世界坐标表示的点P(XW,YW,ZW)与以像素为单位的图像坐标P″(u,v)之间的关系;之后通过对目标图像的处理和分析,提取目标图像特征点,将目标物到摄像机的距离关系转化为具体特征点与摄像机光心之间距离关系,最终建立基于单目视觉的空间测距模型;
第二步,胶囊机器人进入三维非结构封闭空间后,依据被动模态定点悬停调姿,通过控制旋转磁矢量轴线的侧摆角和俯仰角改变其方位,在磁力矩随动效应下,胶囊机器人轴线与旋转磁场轴线始终保持一致,并带动胶囊机器人摄像机在待测区域定点侧摆俯仰扫描观察;当观测到某参考标志时,对获取的图像进行图像处理,提取该参考标志特征的形心并将其作为位置参考点;之后运用单目视觉空间测距模型,计算摄像机光心到位置参考点的距离L1,并从磁场控制器读取摄像机获得参考位置图像时胶囊机器人的侧摆角θ1和俯仰角δ1;
第三步,通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标后再次通过图像处理识别待测目标形心,并根据上述测距方式计算待测目标形心到摄像机光心的距离L2,同时从磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2信息;
第四步,以第二步确定的位置参考点为原点,建立与外部磁场坐标系OXYZ相平行的世界坐标系OwXwYwZw,由于该坐标系可由外部磁场坐标系OXYZ经空间平移得到,且胶囊机器人处于外部磁场的均匀区内,因此胶囊机器人轴线与磁场坐标系OXYZ各坐标轴的夹角和与世界坐标系OwXwYwZw各坐标轴的夹角相同;之后根据胶囊机器人和待测目标与位置参考点距离及空间位置关系,并结合两次观测到目标区域时的俯仰、侧摆角信息,可分别确定出待测目标和胶囊机器人相对于位置参考点的空间方位信息。
上述第一步中,根据小孔成像模型建立单目视觉空间测距模型的推导过程如下:如图1所示,O0为摄像机的光心,O(x0,y0)为摄像机光轴O0Ow与像平面的交点,以O(x0,y0)为原点建立像平面坐标系Oxy,同时以成像面右上角为坐标原点建立与Oxy各坐标轴平行的以像素为单位的坐标系ouv。设摄像机在竖直面内俯仰δ角观测到平面上一目标点P,P′为被测点P在像平面上的映射点,Px'、Py'点分别为P'在x、y轴上的投影点。以光轴与水平面的交点Ow为原点建立二维平面坐标系OwXwYw,点P在各坐标轴上的投影点分别为Px、Py。摄像机光心O0在水平面的投影点为O1,距离水平面的高度为h,摄像机的焦距为f,即OO0=f。
设光轴O0O与O0Py'的夹角为β,与O0Px'的夹角为γ,P'点在坐标系Oxy中的坐标为P'(x1,y1),摄像机光心O0与点P的空间距离为L,则
在三角形O0PPy中,L可表示为
在三角形O0O1Py中,O0Py可表示为
在三角形O0O Py'中,满足
在坐标系OwXwYw中,由投影关系可知PPy=PxOw.,在三角形O0OwPx中
PxOw=O0Ow tanγ (4)
在三角形O0O1Ow中
在三角形O0OPx'中
由式(2)(3)可得
由式(4)(5)(6)可得
由式(1)(7)(8)可得
如图2所示,双半球胶囊机器人的摄像机安装在机器人的顶部,胶囊机器人的半径为R,胶囊轴线在竖直面内的俯仰角为δ,并定义胶囊机器人定点扫描观察时,侧摆和俯仰角逆时针为正,根据图2中的几何关系,可得摄像机光心到水平面的高度h为
h=R(1+sinδ) (10)
由式(9)(10)可得
如果(u,v)为以像素为单位的图像坐标系中的坐标,则O″(u0,v0)为摄像机光轴与像平面交点O(x0,y0)的以像素为单位的坐标,P″(u,v)是P′(x,y)的像素坐标,设像平面上一个像素在x轴和y轴上的物理尺寸分别为dx、dy,则有:
由式(12)可得x=(u-u0)dx,y=(v-v0)dy,令ax=f/dx,ay=f/dy,由式(11)可得被测点P与摄像机光心之间的空间距离为
双半球胶囊机器人球心到摄像机光心的距离近似等于球形机器人半径R,因此可得机器人球心到被测点的空间距离d可表示为
其中,ax,ay,u0和v0为摄像机内部参数,可由摄像机定标确定。
上述第二、三步中,待测目标及胶囊机器人的方位确定过程为:如图3a所示,胶囊机器人在待测区域依据被动模态定点悬停调姿功能,通过控制旋转磁矢量的侧摆角和俯仰角带动胶囊机器人摄像机在P1定点扫描观察,运用图像处理识别某参考标志的形心,并将其作为确定胶囊机器人和待测目标方位的参考点。从磁场控制器读取摄像机获得参考标志图像时胶囊机器人的侧摆角θ1和俯仰角δ1,设该参考标志的形心图像坐标为P1'(u1,v1),根据单目视觉测距原理由式(14)可得胶囊机器人球心到参考点的距离d1为
通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标后再次通过图像处理识别其形心P2,并通过磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2。设待测目标形心的图像坐标为P2'(u2,v2),根据上述方法可计算可得胶囊机器人球心到待测目标形心的距离d2为
以参考标志形心为坐标原点,建立如图3b所示的世界坐标系OwXwYwZw。图3b中,位置参考点Ow和待测目标形心P2在点P1所在的水平面内的投影分别为O2、P2”,设P1点坐标为(X1,Y1,Z1),则由图中几何关系并结合式(15)(16)可得
设P2点坐标为(X2,Y2,Z2),则
综上,胶囊机器人在世界坐标系OwXwYwZw中的实时方位可表示为:
P1(d1cosδ1sinθ1,-d1cosδ1cosθ1,-d1sinδ1)
待测目标的方位可表示为:
P2(d1cosδ1sinθ1-d2cosδ2sinθ2,-d1cosδ1cosθ1+d2cosδ2cosθ2,-d1sinδ1+d2sinδ2)。
本发明的效果和益处是:
该定位方法以磁驱双半球胶囊机器人悬停调姿下的定点全景观察功能为手段,借助磁场与视觉共融的人机交互功能,通过磁矢量方位感知胶囊轴线方位,不仅实现了胶囊机器人在三维非结构空间内实时方位的确定,还实现了该环境中任意待测目标的方位确定。待测目标方位的确定为胶囊机器人针对待测目标的进一步活动提供了必要的技术支撑,胶囊机器人实时位置的确定则为构建完整的主动反馈控制系统奠定了坚实的基础。该种定位方法还解决了影响视觉定位精度的关键难题(诸如摄像机俯仰角和光心高度的实时测量问题),提高了视觉定位的准确性和实时性。其显著特点是无需构建移动机器人传统定位中所需的人工路标和地图,也无需增加其他额外定位检测装置,仅依靠胶囊机器人自身的机器视觉和其定点全景观察功能便可实现其自身方位和任意待测目标方位的确定,该种定位方法具有安全性高,实用性好,环境适应能力强、工作环境要求低,经济可靠的优点。
附图说明
图1是单目视觉空间测距示意图。
图2是胶囊摄像机光心到水平面的高度确定示意图。
图3(a)是胶囊机器人在胃下部区域以幽门作参考点时定位示意图。
图3(b)是以胃幽门作参考点时胶囊机器人与待测目标相对方位确定示意图。
图4(a)胶囊机器人人机交互控制的技术方案示意图。
图4(b)是主被动双半球形胶囊机器人外部结构局部放大图。
图4(c)是主被动双半球形胶囊机器人内部结构局部放大图。
图5(a)是胶囊机器人在胃下部区域以幽门作参考点时定位示意图。
图5(b)是以胃幽门作参考点时胶囊机器人与待测目标相对方位确定示意图。
图中:a患者;b病床;c三轴正交嵌套亥姆霍兹线圈磁场叠加装置;d主被动双半球形胶囊机器人;e1侧摆操纵杆;e2俯仰操纵杆;f信号处理器;g外磁场驱动器;h人机交互界面;nB调姿磁场旋转轴线;
1主动半球壳;2被动半球壳;3轴承定位套筒;4轴承;5圆螺母;6阶梯轴;7径向磁化钕铁硼圆环内驱动器;8摄像头与图像传输装置。
具体实施方式
以下以胃内三维非结构封闭空间的胶囊机器人定位为例,结合技术方案和附图详细叙述具体实施例。
如图4a所示,将三组线圈相互正交嵌套安装成三轴正交嵌套亥姆霍兹线圈磁场叠加装置c,让患者a吞下主被动双半球形胶囊机器人d,并躺在病床b上,调整病床b的位置使患者d的胃部处于三轴正交嵌套亥姆霍兹线圈磁场叠加装置c的中心区域。
实施例1:
当胶囊机器人在胃下部区域时,可按以下步骤实施胶囊机器人和胃内待测目标的定位:
(1)胶囊机器人进入待测区域后(如图3a所示),通过侧摆操纵杆e1与俯仰操纵杆e2使磁矢量轴线侧摆角θ和俯仰角δ分别在0~360°和0~90°内变化,旋转磁矢量与胶囊机器人内嵌径向磁化钕铁硼圆环驱动器相互作用,在磁力矩随动效应下,胶囊机器人轴线始终追随旋转磁矢量轴线改变方位并与其保持一致,从而带动胶囊摄像机在待测区域实现基于定点悬停调姿的全景观察功能。摄像机将拍摄到的实时图像通过无线传输到人机交互界面。当通过扫描观测到胃部幽门管时,根据摄像机拍摄的图像运用基于纹理和暗区相融合的离子群加速算法识别胃幽门管的形心Ow,运用单目视觉测距模型计算摄像机光心到幽门管形心的距离L1,并根据式(14)得到机器人球心P1与幽门管形心Ow的空间距离d1,同时从磁场控制器读取此时胶囊机器人的侧摆角θ1和俯仰角δ1信息。
(2)通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标域后再次通过图像处理识别待测目标形心P2,并根据上述方法计算待测目标形心到机器人球心的距离d2,同时从磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2信息。
(3)以胃幽门管形心Ow为原点,建立与外部磁场坐标系OXYZ相平行的世界坐标系OwXwYwZw,根据胶囊机器人和待测目标与参考点的距离及空间位置关系,并结合两次观测到目标区域时的俯仰、侧摆角信息,可分别确定出胶囊机器人和待测目标相对于幽门管形心的方位信息。
具体定位过程为:如图3b所示,根据胶囊机器人和待测目标与参考点的空间位置关系以及两次观测到目标图像时的俯仰、侧摆角信息,将胶囊机器人到原点Ow的距离d1和到待测目标的距离d2分别向世界坐标系OwXwYwZw的三个坐标轴上投影,由式(17)(18)可分别求得胶囊机器人和待测目标相对胃幽门的相对方位。
实施例2:
当胶囊机器人处于到胃上部区域时,参考实施例1可按以下步骤进行进行定位作业:
(1)胶囊机器人运动到胃上部区域后(如图5a所示),通过侧摆操纵杆e1与俯仰操纵杆e2使磁矢量轴线侧摆角θ和俯仰角δ分别在0~360°和0~90°内变化,旋转磁矢量与胶囊机器人内嵌径向磁化钕铁硼圆环驱动器相互作用,在磁力矩随动效应下,胶囊机器人轴线始终追随旋转磁矢量轴线改变方位并与其保持一致,从而带动胶囊摄像机在胃部实现基于定点悬停调姿的全景观察功能。摄像机将拍摄到时实时图像通过无线传输到人机交互界面。当通过扫描观测到胃部贲门管时,根据图像处理识别胃贲门管的形心,运用单目视觉测距模型,计算摄像机光心到贲门管形心的距离L1,并根据式(14)得到机器人球心P1与贲门管形心Ow的空间距离d1,同时从磁场控制器读取此时胶囊机器人的侧摆角θ1和俯仰角δ1信息。
(2)通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标域后再次通过图像处理识别待测目标形心,并根据上述测距原理计算待测目标形心到机器人球心的距离d2,同时从磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2信息。
(3)以胃贲门管形心为原点,建立与外部磁场坐标系OXYZ相平行的世界坐标系OwXwYwZw,根据机器人球心与贲门管形心和待测目标形心的距离d1、d2及空间位置关系,并结合两次观测到目标区域时的俯仰、侧摆角信息,可分别确定出胶囊机器人和待测目标相对于胃贲门的方位信息。
具体计算过程为:如图5b所示,根据胶囊机器人和待测目标与胃贲门的空间位置关系以及两次观测到目标图像时的俯仰、侧摆角信息,将胶囊机器人到原点Ow的距离d1和到待测目标的距离d2分别向世界坐标系OwXwYwZw的三个坐标轴上投影,由式(17)(18)可分别求得胶囊机器人和待测目标相对胃贲门的方位信息。
Claims (2)
1.三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法,其特征在于,步骤如下:
第一步,首先根据小孔成像模型以及摄像机成像点与目标点的映射关系,建立以世界坐标表示的点P(XW,YW,ZW)与以像素为单位的图像坐标P″(u,v)之间的关系;之后通过对目标图像的处理和分析,提取目标图像特征点,将目标物到摄像机的距离关系转化为具体特征点与摄像机光心之间距离关系,最终建立基于单目视觉的空间测距模型;
第二步,胶囊机器人进入三维非结构封闭空间后,依据被动模态定点悬停调姿,通过控制旋转磁矢量轴线的侧摆角和俯仰角改变其方位,在磁力矩随动效应下,胶囊机器人轴线与旋转磁场轴线始终保持一致,并带动胶囊机器人摄像机在待测区域定点侧摆俯仰扫描观察;当观测到某参考标志时,对获取的图像进行图像处理,提取该参考标志的形心并将其作为位置参考点;之后运用单目视觉空间测距模型,计算摄像机光心到位置参考点的距离L1,并从磁场控制器读取摄像机获得参考位置图像时胶囊机器人的侧摆角θ1和俯仰角δ1;
第三步,通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标后再次通过图像处理识别待测目标形心,并根据上述测距方式计算待测目标形心到摄像机光心的距离L2,同时从磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2信息;
第四步,以第二步确定的位置参考点为原点,建立与外部磁场坐标系OXYZ相平行的世界坐标系OwXwYwZw,由于该坐标系由外部磁场坐标系OXYZ经空间平移得到,且胶囊机器人处于外部磁场的均匀区内,胶囊机器人轴线与磁场坐标系OXYZ各坐标轴的夹角、胶囊机器人轴线与世界坐标系OwXwYwZw各坐标轴的夹角,两个夹角相同;之后根据胶囊机器人和待测目标与位置参考点距离及空间位置关系,并结合两次观测到目标区域时的俯仰、侧摆角信息,分别确定出待测目标和胶囊机器人相对于位置参考点的空间方位信息;
所述的第二、三步中,待测目标及胶囊机器人的方位确定过程为:胶囊机器人在待测区域依据被动模态定点悬停调姿功能,通过控制旋转磁矢量的侧摆角和俯仰角带动胶囊机器人摄像机在P1定点扫描观察,运用图像处理识别某参考标志的形心,并将其作为确定胶囊机器人和待测目标方位的参考点;从磁场控制器读取摄像机获得参考标志图像时胶囊机器人的侧摆角θ1和俯仰角δ1,设该参考标志的形心图像坐标为P1'(u1,v1),根据单目视觉测距原理由式(14)得胶囊机器人球心到该参考标志的形心的距离d1为
通过改变磁矢量轴线方位带动摄像机原地进行待测目标的搜索,当检测到待测目标后再次通过图像处理识别其形心P2,并通过磁场控制器读取此时胶囊机器人的侧摆角θ2和俯仰角δ2;设待测目标形心的图像坐标为P2'(u2,v2),根据上述方法计算得胶囊机器人球心到待测目标形心的距离d2为
其中,ax,ay,u0和v0为摄像机内部参数,可由摄像机定标确定;
以位置参考点Ow为坐标原点,建立世界坐标系OwXwYwZw;位置参考点Ow和待测目标形心P2在点P1所在的水平面内的投影分别为O2、P2”,设P1点坐标为(X1,Y1,Z1),则由图中几何关系并结合式(15)(16)得
设P2点坐标为(X2,Y2,Z2),则
综上,胶囊机器人在世界坐标系OwXwYwZw中的实时方位表示为:
P1(d1cosδ1sinθ1,-d1cosδ1cosθ1,-d1sinδ1)
待测目标的方位表示为:
P2(d1cosδ1sinθ1-d2cosδ2sinθ2,-d1cosδ1cosθ1+d2cosδ2cosθ2,d2sinδ2-d1sinδ1)。
2.根据权利要求1所述的三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法,其特征在于,所述的第一步中,根据小孔成像模型建立单目视觉空间测距模型的推导过程如下:
O0为摄像机的光心,O(x0,y0)为摄像机光轴O0Ow与像平面的交点,以O(x0,y0)为原点建立像平面坐标系Oxy,同时以成像面右上角为坐标原点建立与Oxy各坐标轴平行的以像素为单位的坐标系ouv;设摄像机在竖直面内俯仰δ角观测到平面上一目标点P,P′为被测点P在像平面上的映射点,Px'、Py'点分别为P'在x、y轴上的投影点;以光轴与水平面的交点Ow为原点建立二维平面坐标系OXwYw,点P在各坐标轴上的投影点分别为Px、Py;摄像机光心O0在水平面的投影点为O1,距离水平面的高度为h,摄像机的焦距为f,即OO0=f;
设光轴O0O与O0Py'的夹角为β,与O0 Px'的夹角为γ,P'点在坐标系Oxy中的坐标为P'(x1,y1),摄像机光心O0与点P的空间距离为L,则
在三角形O0PPy中,L表示为
在三角形O0O1Py中,O0Py表示为
在三角形O0OPy'中,满足
在坐标系OwXwYw中,由投影关系知PPy=PxOw,在三角形O0OwPx中
PxOw=O0Owtanγ (4)
在三角形O0O1Ow中
在三角形O0OPx'中
由式(2)(3)得
由式(4)(5)(6)得
由式(1)(7)(8)得
双半球胶囊机器人的摄像机安装在机器人的顶部,胶囊机器人的半径为R,胶囊轴线在竖直面内的俯仰角为δ,并定义胶囊机器人定点扫描观察时,侧摆和俯仰角逆时针为正,根据几何关系,得摄像机光心到水平面的高度h为
h=R(1+sinδ) (10)
由式(9)(10)得
如果(u,v)为以像素为单位的图像坐标系中的坐标,则O″(u0,v0)为摄像机光轴与像平面交点O(x0,y0)的以像素为单位的坐标,P″(u,v)是P′(x,y)的坐标,设像平面上一个像素在x轴和y轴上的物理尺寸分别为dx、dy,则有:
由式(12)得x=(u-u0)dx,y=(v-v0)dy,令ax=f/dx,ay=f/dy,由式(11)得被测点P与摄像机光心之间的空间距离为
双半球胶囊机器人球心到摄像机光心的距离近似等于球形机器人半径R,因此得机器人球心到被测点的空间距离d表示为
其中,ax,ay,u0和v0为摄像机内部参数,由摄像机定标确定。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810285619.XA CN108759812B (zh) | 2018-03-28 | 2018-03-28 | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810285619.XA CN108759812B (zh) | 2018-03-28 | 2018-03-28 | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108759812A CN108759812A (zh) | 2018-11-06 |
CN108759812B true CN108759812B (zh) | 2021-04-20 |
Family
ID=63980677
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810285619.XA Expired - Fee Related CN108759812B (zh) | 2018-03-28 | 2018-03-28 | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108759812B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP7000368B2 (ja) * | 2019-02-08 | 2022-01-19 | ファナック株式会社 | ロボットキャリブレーション方法及びロボットキャリブレーション装置 |
CN109998457B (zh) * | 2019-04-29 | 2021-07-16 | 大连理工大学 | 一种可无线传输图像的主被动双半球高集成胶囊机器人 |
CN110327011B (zh) * | 2019-06-26 | 2021-04-20 | 大连理工大学 | 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 |
CN111982072B (zh) * | 2020-07-29 | 2022-07-05 | 西北工业大学 | 一种基于单目视觉的目标测距方法 |
CN113252007B (zh) * | 2021-06-28 | 2021-09-24 | 常州微亿智造科技有限公司 | 用于工件质检的飞拍控制参数确定方法和装置 |
CN113592934B (zh) * | 2021-06-29 | 2024-02-06 | 浙江工业大学 | 一种基于单目相机的目标深度与高度测量方法及装置 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101297756A (zh) * | 2008-06-19 | 2008-11-05 | 大连理工大学 | 磁场与视觉相结合的医疗微型机器人体内姿态定位方法 |
CN103169445A (zh) * | 2013-04-16 | 2013-06-26 | 苏州朗开医疗技术有限公司 | 一种内窥镜的导航方法及系统 |
CN104983385A (zh) * | 2015-05-21 | 2015-10-21 | 大连理工大学 | 一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法 |
KR20160005817A (ko) * | 2014-07-07 | 2016-01-18 | 서울대학교산학협력단 | 무선 캡슐 내시경을 이용한 의료 영상 시스템 및 이를 위한 의료 영상 처리 방법 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5265179B2 (ja) * | 2007-11-28 | 2013-08-14 | オリンパスメディカルシステムズ株式会社 | カプセル医療システム |
-
2018
- 2018-03-28 CN CN201810285619.XA patent/CN108759812B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101297756A (zh) * | 2008-06-19 | 2008-11-05 | 大连理工大学 | 磁场与视觉相结合的医疗微型机器人体内姿态定位方法 |
CN103169445A (zh) * | 2013-04-16 | 2013-06-26 | 苏州朗开医疗技术有限公司 | 一种内窥镜的导航方法及系统 |
KR20160005817A (ko) * | 2014-07-07 | 2016-01-18 | 서울대학교산학협력단 | 무선 캡슐 내시경을 이용한 의료 영상 시스템 및 이를 위한 의료 영상 처리 방법 |
CN104983385A (zh) * | 2015-05-21 | 2015-10-21 | 大连理工大学 | 一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法 |
Non-Patent Citations (2)
Title |
---|
Navigation of a robotic capsule endoscope with a novel ultrasound tracking system;Jan D.J.Gumprecht et al;《Microsyst Technol》;20130614;第19卷;第1415-1423页 * |
一种新型双半球胶囊机器人;张永顺等;《机械工程学报》;20170831;第53卷(第15期);第110-118页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108759812A (zh) | 2018-11-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108759812B (zh) | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 | |
CN110446159B (zh) | 一种室内无人机精确定位与自主导航的系统及方法 | |
US20200074683A1 (en) | Camera calibration | |
CN105386396B (zh) | 自行式建筑机械以及用于控制自行式建筑机械的方法 | |
CN106625673A (zh) | 狭小空间装配系统及装配方法 | |
JP4132068B2 (ja) | 画像処理装置及び三次元計測装置並びに画像処理装置用プログラム | |
JP7153306B2 (ja) | 検出対象位置特定システム | |
CN111247389B (zh) | 关于拍摄设备的数据处理方法、装置及图像处理设备 | |
CN111551171A (zh) | 目标对象的定位方法、装置、机器人和存储介质 | |
US11826015B2 (en) | System and method for controlling capsule endoscope | |
CN110998241A (zh) | 用于校准可移动对象的光学系统的系统和方法 | |
Núnez et al. | Data Fusion Calibration for a 3D Laser Range Finder and a Camera using Inertial Data. | |
WO2019183789A1 (zh) | 无人机的控制方法、装置和无人机 | |
WO2019189381A1 (ja) | 移動体、制御装置、および制御プログラム | |
CN103226693A (zh) | 基于全景立体视觉的捕捞对象的识别和空间定位装置及方法 | |
Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
TWI274971B (en) | Mark-assisted positioning system and method | |
JP4918675B2 (ja) | 3次元座標測定方法 | |
WO2020024150A1 (zh) | 地图处理方法、设备、计算机可读存储介质 | |
CN110490934A (zh) | 基于单目相机与机器人的混合机立式桨叶姿态检测方法 | |
Cao et al. | Omni-directional vision localization based on particle filter | |
CN107330936A (zh) | 一种基于单目视觉的双圆形标志物定位方法及系统 | |
CN113733078B (zh) | 机械臂精调控制量判读方法、计算机可读存储介质 | |
CN116867611A (zh) | 融合静态大视场和高保真度可移动传感器用于机器人平台 | |
Liu et al. | An image-based accurate alignment for substation inspection robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210420 |
|
CF01 | Termination of patent right due to non-payment of annual fee |