CN108827300A - 一种基于视觉的装备姿态位置测量方法及系统 - Google Patents
一种基于视觉的装备姿态位置测量方法及系统 Download PDFInfo
- Publication number
- CN108827300A CN108827300A CN201810341530.0A CN201810341530A CN108827300A CN 108827300 A CN108827300 A CN 108827300A CN 201810341530 A CN201810341530 A CN 201810341530A CN 108827300 A CN108827300 A CN 108827300A
- Authority
- CN
- China
- Prior art keywords
- projection
- feature
- plane
- target
- extracted
- 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.)
- Pending
Links
- 238000000691 measurement method Methods 0.000 title claims abstract description 15
- 238000000034 method Methods 0.000 claims abstract description 114
- 238000012545 processing Methods 0.000 claims abstract description 31
- 238000003745 diagnosis Methods 0.000 claims abstract description 15
- 238000000605 extraction Methods 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000004364 calculation method Methods 0.000 claims description 25
- 238000006073 displacement reaction Methods 0.000 claims description 24
- 238000005259 measurement Methods 0.000 claims description 22
- 238000003384 imaging method Methods 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 5
- 239000002245 particle Substances 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 5
- 238000012163 sequencing technique Methods 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims 1
- 239000000284 extract Substances 0.000 abstract description 2
- 238000004422 calculation algorithm Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- 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
- G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于视觉的装备姿态位置测量方法及系统,该系统包括:图像获取子系统、图像处理子系统和位姿解算子系统,其中图像获取子系统用于控制图像获取设备获取装备的图像;图像处理子系统用于对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外;位姿解算子系统用于解算装备的姿态位置。发明的方法和系统能够保证装备的姿态位置测量方法在长时间工作后的高精度,降低装备的结构复杂度。
Description
技术领域
本发明涉及计算机视觉领域,尤其涉及一种基于视觉的装备姿态位置测量方法及系统。
背景技术
许多装备在使用过程中并不是静止不动的,以雷达为例,雷达是各种武器系统与作战指挥控制系统的最重要的组成部分之一,当雷达处于跟踪状态时,天线的空间指向角将随着目标的运动而变化,因此,天线基座的姿态测量结果将决定着雷达是否能够正常工作。
在装备性能试验过程中,也需要对装备的姿态和位置进行测量,传统的装备姿态位置测量方法多为机械方式,其在长时间工作之后,难以保证高精度的要求,并且在一定程度上增加了装备的结构复杂度。高精度陀螺仪是一种常用的姿态测量仪器,但是其具有价格昂贵,标定复杂等缺点,不适合用于装备性能的试验实验中。
发明内容
本发明所要解决的技术问题是:针对现有技术中的问题,提出一种基于视觉的装备姿态位置测量方法及系统,降低了机械方式测量装备的姿态位置的装备结构复杂度,并保证了长时间工作后的精度。
本发明提供的一种基于视觉的装备姿态位置测量方法,包括:
图像获取设备获取装备的图像;
对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外;
解算装备的姿态位置。
进一步,所述特征靶标为五个,其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外,且平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备。
进一步,特征投影提取的具体方法包括:首先将图像进行灰度处理,然后通过设置阀值将灰度图转换成二值图像,之后进行粒子滤波,并根据设置的像素面积大小对特征靶标的投影进行识别,并提取特征靶标投影的中心坐标。
进一步,特征投影匹配和故障诊断的具体方法包括:
判断提取到的投影个数;
若提取到的投影个数为5,则执行以下步骤:
步骤11,利用方法一对平面外的特征靶标的投影进行匹配;
步骤12,利用方法二对平面内的特征靶标的投影进行匹配;
若提取到的投影个数为4,则执行以下步骤:
步骤21,利用方法一判断平面外的特征靶标的投影是否提取失败并完成该投影的匹配;
步骤22,若平面外的特征靶标的投影提取失败,则利用方法二对平面内的特征靶标的投影进行匹配;
步骤23,若平面外的特征靶标的投影正常提取,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
若提取到的投影个数为3,则执行以下步骤:
步骤31,利用方法一判断平面外的特征靶标的投影是否提取失败;
步骤32,若平面外的特征靶标的投影提取失败,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
步骤33,若平面外的特征靶标的投影正常提取,则结束该过程;
若提取到的投影个数小于3,则结束该过程。
进一步,方法一为:比较提取的投影的像素面积大小,若某个投影的像素面积远大于其他投影的像素面积,则该投影为平面外的特征靶标的投影。
进一步,方法二为:对平面内的特征靶标构成的4个三角形的面积大小及平面内特征靶标的投影构成4个三角形的面积大小分别进行排序,根据平面内的特征靶标构成的三角形在投影前后其面积大小排序不变的原理对平面内的特征靶标的投影进行匹配。
进一步,方法三为:将上一时刻4个平面内特征靶标的投影中心坐标与当前时刻正常提取的3个平面内特征靶标的投影中心坐标分别计算相对距离,共 12组数据,利用相对距离最小的3组数据完成当前时刻正常提取投影与上一时刻特征靶标投影的对应关系,完成特征靶标投影的匹配,确定故障投影对应的特征靶标序号。
进一步,装备姿态位置的解算方法包括:
步骤1,设置景深dn的初始值所述景深dn为特征靶标Pc,n距离成像平面的距离,其中Pc,n=[xc,n,yc,n,zc,n]T为特征靶标在图像获取设备坐标系 Oc-XcYcZc中的坐标,n=1,2,...N,N为特征靶标的个数;
步骤2,计算Pc,n,其中投影正常提取的特征靶标的Pc,n的计算方法为: Pc,n k=dn kvn,式中,k表示当前迭代次数,其中,f为图像获取设备的焦距,xi,n,yi,n为特征靶标投影的中心在图像平面坐标系Oi-XiYi中的坐标,且Pi,n=[xi,n,yi,n]T;
投影提取故障的平面内特征靶标的Pc,n的计算方法为:
式中,下标g表示投影提取故障的平面内特征靶标,下标a、b和e表示投影提取正常的平面内特征靶标;
步骤3,根据Pc,n和Pt,n计算旋转矩阵R和位移矢量T,其中,Pt,n=[xt,n,yt,n,zt,n]T为特征靶标在装备体坐标系Ot-XtYtZt中的坐标,R和T分别表示装备体坐标系相对于图像获取设备坐标系的旋转和平移,即满足 Pc,n=RPt,n+T;
步骤4,计算误差其中,
步骤5,对景深dn进行迭代,其中,投影正常提取的特征靶标的迭代方法为:投影提取故障的平面内特征靶标的迭代方法为:
步骤6,若两次迭代误差之差小于设定的阀值则停止迭代,输出旋转矩阵R和位移矢量T,并计算当前时刻提取失败的平面内特征靶标投影的理论投影坐标。
进一步,步骤3中位移矢量T的计算方法为:
本发明的另一方面提供了一种基于视觉的装备姿态位置测量系统,其特征在于,包括:
图像获取子系统,用于控制图像获取设备获取装备的图像;
图像处理子系统,用于对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外;
位姿解算子系统,用于解算装备的姿态位置。
进一步,所述特征靶标为五个,其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外,且平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备。
进一步,图像处理子系统提取特征投影的具体方法包括:首先将图像进行灰度处理,然后通过设置阀值将灰度图转换成二值图像,之后进行粒子滤波,并根据设置的像素面积大小对特征靶标的投影进行识别,并提取特征靶标投影的中心坐标。
进一步,图像处理子系统进行特征投影匹配和故障诊断的具体方法包括:
判断提取到的投影个数;
若提取到的投影个数为5,则执行以下步骤:
步骤11,利用方法一对平面外的特征靶标的投影进行匹配;
步骤12,利用方法二对平面内的特征靶标的投影进行匹配;
若提取到的投影个数为4,则执行以下步骤:
步骤21,利用方法一判断平面外的特征靶标的投影是否提取失败并完成该投影的匹配;
步骤22,若平面外的特征靶标的投影提取失败,则利用方法二对平面内的特征靶标的投影进行匹配;
步骤23,若平面外的特征靶标的投影正常提取,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
若提取到的投影个数为3,则执行以下步骤:
步骤31,利用方法一判断平面外的特征靶标的投影是否提取失败;
步骤32,若平面外的特征靶标的投影提取失败,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
步骤33,若平面外的特征靶标的投影正常提取,则结束该过程;
若提取到的投影个数小于3,则结束该过程。
进一步,方法一为:比较提取的投影的像素面积大小,若某个投影的像素面积远大于其他投影的像素面积,则该投影为平面外的特征靶标的投影。
进一步,方法二为:对平面内的特征靶标构成的4个三角形的面积大小及平面内特征靶标的投影构成4个三角形的面积大小分别进行排序,根据平面内的特征靶标构成的三角形在投影前后其面积大小排序不变的原理对平面内的特征靶标的投影进行匹配。
进一步,方法三为:将上一时刻4个平面内特征靶标的投影中心坐标与当前时刻正常提取的3个平面内特征靶标的投影中心坐标分别计算相对距离,共 12组数据,利用相对距离最小的3组数据完成当前时刻正常提取投影与上一时刻特征靶标投影的对应关系,完成特征靶标投影的匹配,确定故障投影对应的特征靶标序号。
进一步,图像处理子系统进行装备姿态位置的解算方法包括:
步骤1,设置景深dn的初始值所述景深dn为特征靶标Pc,n距离成像平面的距离,其中Pc,n=[xc,n,yc,n,zc,n]T为特征靶标在图像获取设备坐标系 Oc-XcYcZc中的坐标,n=1,2,...N,N为特征靶标的个数;
步骤2,计算Pc,n,其中投影正常提取的特征靶标的Pc,n的计算方法为: Pc,n k=dn kvn,式中,k表示当前迭代次数,其中,f为图像获取设备的焦距,xi,n,yi,n为特征靶标投影的中心在图像平面坐标系Oi-XiYi中的坐标,且Pi,n=[xi,n,yi,n]T;
投影提取故障的平面内特征靶标的Pc,n的计算方法为:
式中,下标g表示投影提取故障的平面内特征靶标,下标a、b和e表示投影提取正常的平面内特征靶标;
步骤3,根据Pc,n和Pt,n计算旋转矩阵R和位移矢量T,其中, Pt,n=[xt,n,yt,n,zt,n]T为特征靶标在装备体坐标系Ot-XtYtZt中的坐标,R和T分别表示装备体坐标系相对于图像获取设备坐标系的旋转和平移,即满足 Pc,n=RPt,n+T;
步骤4,计算误差其中,
步骤5,对景深dn进行迭代,其中,投影正常提取的特征靶标的迭代方法为:投影提取故障的平面内特征靶标的迭代方法为:
步骤6,若两次迭代误差之差小于设定的阀值则停止迭代,输出旋转矩阵R和位移矢量T,并计算当前时刻提取失败的平面内特征靶标投影的理论投影坐标。
进一步,步骤3中位移矢量T的计算方法为:
与现有技术相比,本发明能够保证装备的姿态位置测量方法在长时间工作后的高精度,降低装备的结构复杂度。在保证装备姿态位置测量精度的同时,还可以作为一种经济简易的装备研制过程中姿态位置的测量方法。
附图说明
本发明将通过例子并参照附图的方式说明,其中:
图1为装备姿态位置测量系统的原理框图;
图2为特征投影匹配与故障诊断的流程图;
图3为图像平面坐标系、图像获取设备坐标系和装备体坐标系之间的关系示意图;
图4为共面特征靶标的示意图;
图5为本发明的姿态位置解算方法流程图;
图6为与本发明的具体实施例一致的装备姿态位置测量系统的总体框图。
具体实施方式
本说明书中公开的所有特征,或公开的所有方法或过程中的步骤,除了互相排斥的特征和/或步骤以外,均可以以任何方式组合。
本说明书中公开的任一特征,除非特别叙述,均可被其他等效或具有类似目的的替代特征加以替换。即,除非特别叙述,每个特征只是一系列等效或类似特征中的一个例子而已。
本发明公开了一种基于视觉的装备姿态位置测量方法及系统,该系统包括图像获取子系统、图像处理子系统和位姿解算子系统,如图1所示。该系统的主要功能包括:
(1)控制图像获取设备获取装备的图像;
(2)特征投影提取、特征投影匹配和故障诊断等;
(3)对装备姿态位置进行解算。
a.图像获取子系统
图像获取子系统用于控制图像获取设备进行图像的采集,数据存储输出子系统用于对数据进行保存并完成各子系统数据的交换。在一些实施例中,图像获取设备为工业相机。
b.图像处理子系统
图像处理子系统用于对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外。在一些实施例中,特征靶标为五个,其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外,且平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备。
在一些实施例中,特征投影提取的具体方法为:首先将图像进行灰度处理,然后通过设置阀值将灰度图转换成二值图像,之后进行粒子滤波,并根据设置的像素面积大小对特征靶标的投影进行识别,并提取特征靶标投影的中心坐标。
特征投影提取只能完成对图像上特征投影的识别并获取投影的中心坐标,但是并不知道某个特征投影对应着现实中的哪一个特征靶标,因此需要进行特征投影匹配。特征投影匹配的方法与提取的特征投影的数目有关,在正常情况下,五个投影靶标可以提取五个特征投影,但是如果发生特征靶标失效、特征投影提取出现故障等情况,可能会导致五个特征靶标的投影未全部识别或者提取,从而之后利用较少的特征投影进行姿态位置解算会使得精度下降甚至结果错误,因此本系统加入了故障诊断功能。特征投影匹配与故障诊断的具体原理和过程如图2所示:
(1)当提取到5个特征投影时,则正常提取了所有特征靶标的投影,将特征靶标的中心在装备体坐标系中的坐标定义为Pt,n,其中Pt,n(n=1,2,3,4)和Pt,5分别为平面内和平面外的特征靶标,将特征靶标对应的投影中心在图像平面坐标系中的坐标定义为Pi,n(n=1,2,3,4,5)。
(1.1)对平面外的特征靶标的投影进行匹配。由于平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备,因此其投影的像素面积远大于平面内的特征靶标的投影的像素面积。在一些实施例中,比较提取的投影的像素面积大小,若某个投影的像素面积远大于其他投影的像素面积,则该投影为平面外的特征靶标的投影。
(1.2)对平面内的特征靶标的投影进行匹配。对于平面内的特征靶标,每3 个特征靶标构成1个三角形,共有4个三角形,并将其定义为:△Pt,1Pt,2Pt,3,△Pt,1Pt,2Pt,4,△Pt,1Pt, 3Pt,4和△Pt,2Pt,3Pt,4。对这4个三角形的面积大小进行排序,假设4个三角形的面积大小关系满足:
同样,平面内的特征靶标的投影也构成4个三角形,将其定义为:△Pi,1Pi,2Pi,3,△Pi,1Pi,2Pi,4,△Pi,1Pi,3Pi,4和△Pi,2Pi,3Pi,4。则投影Pi,n(n=1,2,3,4)构成的三角形面积大小关系满足:
由式(1)和式(2)可知,平面内的特征靶标构成的三角形在投影前后其面积大小排序不变,从而根据该原理对平面内的特征靶标的投影进行匹配。
(2)当提取到4个投影时,则有1个特征靶标的投影提取失败,需要确定故障投影对应的特征靶标序号和对正常提取的投影进行匹配。
(2.1)判断平面外的特征靶标的投影是否提取失败。由于平面外的特征靶标的投影像素面积远大于平面内的特征靶标的投影像素面积,因此将提取的4个投影的像素面积进行比较,如果有1个投影的像素面积远大于其余3个投影的像素面积,则这个投影为平面外的特征靶标的投影,从而也完成了对平面外的特征靶标的投影的匹配。
(2.2)如果提取失败的是平面外的特征靶标的投影,则正常提取了4个平面内的特征靶标的投影,根据步骤(1.2)中的三角形面积排序法对平面内的特征靶标的投影进行匹配。
(2.3)如果平面外的特征靶标的投影Pi,5没有提取失败,则有1个平面内的特征靶标的投影提取失败。考虑平面内不同的特征靶标的投影在同一个时刻具有分散性,同时,同一个特征靶标的投影在两个相邻时刻具有紧密性,从而来确定提取失败的投影对应的特征靶标序号并匹配正常提取的特征靶标的投影。具体来说,分散性是由于平面内不同的特征靶标在武器装备台面上的位置分布零散且互相距离较远,因此它们在图像平面上的投影同样分布零散且相互距离较远。紧密性是由于武器装备的运动在相邻两个时刻是连续变化的,因此同一个特征靶标的投影在两个相邻时刻的投影位置是紧挨着的。利用分散性与紧密性,将平面内特征靶标的上一时刻投影的中心坐标与当前时刻正常提取的3个投影的中心坐标分别计算相对距离,共12组数据,利用相对距离最小的3组数据完成该时刻正常提取投影与上一时刻特征靶标投影的对应关系,从而完成了特征靶标投影的匹配,也确定了故障投影对应的特征靶标序号。
注意:当上一时刻平面内特征靶标的投影全部正常提取与匹配时,则将该时刻的投影坐标直接应用到步骤(2.3)中相对距离的计算。当上一时刻有平面内特征靶标的投影提取失败,则需要在之后的位姿解算子系统中获取上一时刻提取失败的平面内特征靶标的投影中心理论坐标,从而完成步骤(2.3)中相对距离的计算。
(3)当提取到3个投影时,则有2个投影提取失败,需要确定故障投影对应的特征靶标序号和对正常提取的投影进行匹配。
(3.1)同步骤(2.1)判断平面外的特征靶标的投影是否提取失败。
(3.2)如果平面外的特征靶标的投影提取失败,则有1个平面内的特征靶标的投影提取失败。同步骤(2.3)匹配正常提取的3个平面内的特征靶标的投影。
(3.3)如果平面外的特征靶标的投影Pi,5正常提取,则有2个平面内的特征靶标的投影提取失败,无法完成武器装备姿态位置的解算。
(4)当提取的投影少于3个投影时,无法完成武器装备姿态位置的解算。
当完成了图像获取、特征投影识别与提取、特征投影匹配与故障诊断等工作,之后进行武器装备的姿态位置解算。
c.位姿解算子系统
位姿解算子系统的主要功能是:利用特征靶标的坐标和其投影坐标来完成装备姿态位置的解算。
为了描述装备的姿态位置,首先定义三个坐标系:图像平面坐标系 (Oi-XiYi)、图像获取设备坐标系(Oc-XcYcZc)和装备体坐标系(Ot-XtYtZt)。三个坐标系之间的关系如3图所示。
图像平面坐标系(Oi-XiYi)是用来描述特征靶标投影到成像平面上的投影在成像平面中的物理坐标,其原点Oi位于图像获取设备主轴与成像平面的交点, Xi轴和Yi轴分别平行于成像平面的水平方向和垂直方向,特征靶标投影的中心在该坐标系中的坐标定义为Pi,n=[xi,n,yi,n]T(n=1,2,...)。
图像获取设备坐标系(Oc-XcYcZc)的原点Oc位于图像获取设备中心,Zc轴垂直于成像平面,Xc轴和Yc轴分别平行于图像平面坐标系的Xi轴和Yi轴,特征靶标在该坐标系中的坐标定义为Pc,n=[xc,n,yc,n,zc,n]T(n=1,2,...)。
装备体坐标系(Ot-XtYtZt)固连在装备上,通常坐标系的原点Ot位于装备的转动中心,三个坐标轴的方向满足右手螺旋规则即可,特征靶标在该坐标系中的坐标定义为Pt,n=[xt,n,yt,n,zt,n]T(n=1,2,...)。
装备的姿态位置实际是指装备体坐标系相对于图像获取设备坐标系的旋转和平移,分别用旋转矩阵R3×3和位移矢量T3×1来描述,即满足:
Pc,n=RPt,n+T (3)
在实际中,可以获取的参数是特征靶标在装备体坐标系的坐标Pt,n和对应投影在成像平面坐标系上的坐标Pi,n。但是在解算的过程中,需要利用绝对定位算法来做计算的支撑。
1.绝对定位算法
绝对定位算法的输入是:特征靶标在图像获取设备坐标系的坐标Pc,n和在装备体坐标系的坐标Pt,n。输出是:装备体坐标系相对于图像获取设备坐标系的旋转矩阵R和位移矢量T。
定义坐标位置误差为δp,由式(3)可得其具体表达式为:
式中,N为特征靶标的个数。由上式可知,通过最小化坐标位置误差来求得旋转矩阵R和位移矢量T。有如下定义:
Qt,n=Pt,n-Pt,n=1,2...N (7)
Qc,n=Pc,n-Pc,n=1,2...N (8)
将以上定义代入式(4)可得:
上式整理可得:
那么,将坐标位置误差最小化等同于最大化:
式中,H矩阵的具体表达式为:
进行奇异值分解H=UΛVT,U矩阵和V矩阵为3×3阶正交矩阵,Λ矩阵为3×3阶的非负对角矩阵。定义正交矩阵X=VUT,可得:
XH=VUTUΛVT=VΛVT (13)
式中,VΛV为正定对称矩阵。
假设X=VUT不是所求旋转矩阵R,则存在一个旋转矩阵(正交矩阵)B使其做一次旋转,使得BX为所求的旋转矩阵R。根据定理:对于任意正定矩阵 AAT和任意正交矩阵B,都有Trace(AAT)≥Trace(BAAT)成立,那么 Trace(XH)≥Trace(BXH)成立,因此X=VUT能够使得F=Trace(RH)最大化,即X=VUT为所求旋转矩阵R,再由下式计算位移矢量T:
T=Pc-RPt (14)
2.传统姿态位置解算方法
姿态位置解算方法的输入是:特征靶标在装备体坐标系上的坐标Pt,n和对应投影在成像平面坐标系上的坐标Pi,n;输出是:装备体坐标系相对于图像获取设备坐标系的旋转矩阵R和位移矢量T。
如图3所示,当没有投影误差时,特征靶标Pc,n应该位于从图像获取设备中心Oc指向图像平面对应投影Pi,n的射线上,射线OcPi,n的方向矢量vn为:
式中,f为图像获取设备的焦距,N为特征靶标的数目。
定义特征靶标Pc,n距离成像平面的距离为景深dn,即dn=zc,n,那么满足:
Pc,n=dnvn,n=1,2...N (16)
由于存在投影误差,那么由式(3)与式(16)表示的特征靶标坐标Pc,n存在一定的误差,定义该误差为:
采用迭代法对景深dn、旋转矩阵R和位移矢量T进行迭代,在迭代过程中,变量符号上标k表示当前迭代次数。首先设置初始景深值然后根据以下迭代步骤进行计算:
(1)基于dn通过式(16)求得Pc,n,再基于Pc,n和Pt,n根据绝对定位算法求解R 和T,并计算误差
(2)利用以下公式对dn进行迭代:
当两次迭代误差之差小于设定的阀值时停止迭代,然后输出旋转矩阵 R和位移矢量T。
3.改进的姿态位置解算方法
当某一投影提取失败,那么无法通过式(15)求得射线方向矢量vn,继而无法对景深进行迭代,导致无法完成姿态位置的解算。考虑特征靶标之间坐标Pc,n具有相对的位置关系,又由于dn=zc,n,因此利用其它特征靶标的景深来完成故障特征靶标景深的迭代。考虑这种迭代方法会严重影响算法的整体迭代速度,因此优化了方法的迭代速度。
对景深迭代的迭代速度优化如下:
由式(17)可得第k次迭代的误差可表示为:
式中,vn′为射线OcPi,n的单位矢量,即vn′=vn|vn|-1。由上式可知,为关于旋转矩阵Rk和位移矢量Tk的函数,对上式关于位移矢量Tk求偏导,并令其等于零,可得:
进一步推导可得:
式中,I为3×3阶单位矩阵。对任意3×1阶矢量Γ,有下式成立:
由于vn′为射线OcPi,n的单位矢量,所以vn′TΓ为矢量Γ在射线OcPi,n上的投影,因此||Γ||2-||vn′TΓ||2≥0且只有当矢量Γ与射线OcPi,n平行时才相等。考虑所有射线OcPi,n不可能平行,因此矢量Γ不可能与所有射线OcPi,n平行,所以有下式成立:
由矩阵正定性判定定理可知,为3×3阶正定矩阵,因此其可逆,由式(21)可得当前迭代的最优位移矢量为:
通过求解最优位移矢量可以提高误差的减小速度,从而提高了算法的迭代速度。
针对故障投影的景深迭代方法如下:
四个平面内的特征靶标Pt,n=[xt,n,yt,n,zt,n]T(n=1,2,3,4)的分布情况如图4所示,Pt,6为四个特征靶标连线的交点。假设投影Pi,4提取失败,其它投影 Pi,n(n=1,2,3)正常提取,此时无法通过式(15)和式(16)计算迭代过程中的Pc,4,改用下式计算:
式中,λ13和λ24为坐标比例因子,其分别为:
由于dn=zc,n和Pc,n=[xc,n,yc,n,zc,n]T,那么由式(25)可得:
从而完成故障投影的景深d4 k+1的迭代。
需要注意的是:这种对故障投影的景深迭代方法仅适用于提取的三个投影均为平面内特征靶标的投影。
图5为本发明的姿态位置解算方法流程图。如图5所示,首先初始化参数,包括景深参数;然后判断是否存在投影提取故障,对正常提取的投影采用传统的景深迭代方法,对提取故障的投影采用改进的景深迭代方法;之后,基于此次迭代的景深值采用绝对定位算法计算出旋转矩阵,再采用改进的位姿解算算法计算出最优位移矢量;最后,计算误差并判断其大小是否满足误差要求,如果不满足误差要求则继续对景深进行迭代,直至满足误差要求为止,当满足误差要求便输出装备的姿态和位置,
本发明的姿态位置解算方法比传统的姿态位置解算方法具有更快的收敛性,除此之外,传统的方法最少需要正确提取了四个特征靶标投影才能完成解算,而本发明的方法能基于三个平面内的特征靶标的投影完成姿态位置的解算,增加了系统的鲁棒性。
在一个具体实施例中,在笔记本电脑上运行有姿态位置测量系统和装备状态显示终端,如图6所示。由姿态位置测量系统发出指令通过网线控制工业相机开始采集装备的图像,并通过网线将图像数据传输到姿态位置测量系统中,在姿态位置测量系统中完成图像处理、姿态位置解算的过程,最后姿态位置测量系统将姿态位置解算结果传输到装备状态显示终端。
在姿态位置测量系统中,对图像处理过程包括:特征投影提取、特征投影匹配和故障诊断等过程。
装备:在装备上布置五个特征靶标,并且其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外。
工业相机:工业相机用来获取装备的图像。
姿态位置测量系统:将工业相机获取的图像进行处理,包括识别并提取特征靶标的投影,对装备的姿态位置进行解算,并将数据传输到装备状态显示终端。
装备状态显示终端:显示装备的姿态位置数据,方便监视装备的工作状态。
姿态位置测量系统:该系统主要包括图像获取子系统、图像处理子系统和位姿解算子系统等。
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合,以及披露的任一新的方法或过程的步骤或任何新的组合。
Claims (19)
1.一种基于视觉的装备姿态位置测量方法,其特征在于,包括:
控制图像获取设备获取装备的图像;
对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外;
解算装备的姿态位置。
2.根据权利要求1所述的一种基于视觉的装备姿态位置测量方法,其特征在于,所述特征靶标为五个,其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外,且平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备。
3.根据权利要求1所述的一种基于视觉的装备姿态位置测量方法,其特征在于,特征投影提取的具体方法包括:首先将图像进行灰度处理,然后通过设置阀值将灰度图转换成二值图像,之后进行粒子滤波,并根据设置的像素面积大小对特征靶标的投影进行识别,并提取特征靶标投影的中心坐标。
4.根据权利要求2所述的一种基于视觉的装备姿态位置测量方法,其特征在于,特征投影匹配和故障诊断的具体方法包括:
判断提取到的投影个数;
若提取到的投影个数为5,则执行以下步骤:
步骤11,利用方法一对平面外的特征靶标的投影进行匹配;
步骤12,利用方法二对平面内的特征靶标的投影进行匹配;
若提取到的投影个数为4,则执行以下步骤:
步骤21,利用方法一判断平面外的特征靶标的投影是否提取失败并完成该投影的匹配;
步骤22,若平面外的特征靶标的投影提取失败,则利用方法二对平面内的特征靶标的投影进行匹配;
步骤23,若平面外的特征靶标的投影正常提取,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
若提取到的投影个数为3,则执行以下步骤:
步骤31,利用方法一判断平面外的特征靶标的投影是否提取失败;
步骤32,若平面外的特征靶标的投影提取失败,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
步骤33,若平面外的特征靶标的投影正常提取,则结束该过程;
若提取到的投影个数小于3,则结束该过程。
5.根据权利要求4所述的一种基于视觉的装备姿态位置测量方法,其特征在于,方法一为:比较提取的投影的像素面积大小,若某个投影的像素面积远大于其他投影的像素面积,则该投影为平面外的特征靶标的投影。
6.根据权利要求4所述的一种基于视觉的装备姿态位置测量方法,其特征在于,方法二为:对平面内的特征靶标构成的4个三角形的面积大小及平面内特征靶标的投影构成4个三角形的面积大小分别进行排序,根据平面内的特征靶标构成的三角形在投影前后其面积大小排序不变的原理对平面内的特征靶标的投影进行匹配。
7.根据权利要求4所述的一种基于视觉的装备姿态位置测量方法,其特征在于,方法三为:将上一时刻4个平面内特征靶标的投影中心坐标与当前时刻正常提取的3个平面内特征靶标的投影中心坐标分别计算相对距离,共12组数据,利用相对距离最小的3组数据完成当前时刻正常提取投影与上一时刻特征靶标投影的对应关系,完成特征靶标投影的匹配,确定故障投影对应的特征靶标序号。
8.根据权利要求4所述的一种基于视觉的装备姿态位置测量方法,其特征在于,装备姿态位置的解算方法包括:
步骤1,设置景深dn的初始值所述景深dn为特征靶标Pc,n距离成像平面的距离,其中Pc,n=[xc,n,yc,n,zc,n]T为特征靶标在图像获取设备坐标系Oc-XcYcZc中的坐标,n=1,2,...N,N为特征靶标的个数;
步骤2,计算Pc,n,其中投影正常提取的特征靶标的Pc,n的计算方法为:Pc,n k=dn kvn,式中,k表示当前迭代次数,其中,f为图像获取设备的焦距,xi,n,yi,n为特征靶标投影的中心在图像平面坐标系Oi-XiYi中的坐标,且Pi,n=[xi,n,yi,n]T;
投影提取故障的平面内特征靶标的Pc,n的计算方法为:
式中,下标g表示投影提取故障的平面内特征靶标,下标a、b和e表示投影提取正常的平面内特征靶标;
步骤3,根据Pc,n和Pt,n计算旋转矩阵R和位移矢量T,其中,Pt,n=[xt,n,yt,n,zt,n]T为特征靶标在装备体坐标系Ot-XtYtZt中的坐标,R和T分别表示装备体坐标系相对于图像获取设备坐标系的旋转和平移,即满足Pc,n=RPt,n+T;
步骤4,计算误差其中,
步骤5,对景深dn进行迭代,其中,投影正常提取的特征靶标的迭代方法为:投影提取故障的平面内特征靶标的迭代方法为:
步骤6,若两次迭代误差之差小于设定的阀值则停止迭代,输出旋转矩阵R和位移矢量T,并计算当前时刻提取失败的平面内特征靶标投影的理论投影坐标。
9.根据权利要求8所述的一种基于视觉的装备姿态位置测量方法,其特征在于,步骤3中位移矢量T的计算方法为:
10.一种基于视觉的装备姿态位置测量系统,其特征在于,包括:
图像获取子系统,用于控制图像获取设备获取装备的图像;
图像处理子系统,用于对图像进行处理,处理过程包括特征靶标的特征投影提取、特征投影匹配和故障诊断,所述特征靶标布置在装备上,且其中部分特征靶标在同一个平面内,部分特征靶标在该平面外;
位姿解算子系统,用于解算装备的姿态位置。
11.根据权利要求10所述的一种基于视觉的装备姿态位置测量系统,其特征在于,所述特征靶标为五个,其中四个特征靶标在同一个平面内,另一个特征靶标在该平面外,且平面外的特征靶标相对于平面内的特征靶标更靠近图像获取设备。
12.根据权利要求10所述的一种基于视觉的装备姿态位置测量系统,其特征在于,图像处理子系统提取特征投影的具体方法包括:首先将图像进行灰度处理,然后通过设置阀值将灰度图转换成二值图像,之后进行粒子滤波,并根据设置的像素面积大小对特征靶标的投影进行识别,并提取特征靶标投影的中心坐标。
13.根据权利要求11所述的一种基于视觉的装备姿态位置测量系统,其特征在于,图像处理子系统进行特征投影匹配和故障诊断的具体方法包括:
判断提取到的投影个数;
若提取到的投影个数为5,则执行以下步骤:
步骤11,利用方法一对平面外的特征靶标的投影进行匹配;
步骤12,利用方法二对平面内的特征靶标的投影进行匹配;
若提取到的投影个数为4,则执行以下步骤:
步骤21,利用方法一判断平面外的特征靶标的投影是否提取失败并完成该投影的匹配;
步骤22,若平面外的特征靶标的投影提取失败,则利用方法二对平面内的特征靶标的投影进行匹配;
步骤23,若平面外的特征靶标的投影正常提取,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
若提取到的投影个数为3,则执行以下步骤:
步骤31,利用方法一判断平面外的特征靶标的投影是否提取失败;
步骤32,若平面外的特征靶标的投影提取失败,则利用方法三对正常提取的平面内的特征靶标的投影进行匹配;
步骤33,若平面外的特征靶标的投影正常提取,则结束该过程;
若提取到的投影个数小于3,则结束该过程。
14.根据权利要求13所述的一种基于视觉的装备姿态位置测量系统,其特征在于,方法一为:比较提取的投影的像素面积大小,若某个投影的像素面积远大于其他投影的像素面积,则该投影为平面外的特征靶标的投影。
15.根据权利要求13所述的一种基于视觉的装备姿态位置测量系统,其特征在于,方法二为:对平面内的特征靶标构成的4个三角形的面积大小及平面内特征靶标的投影构成4个三角形的面积大小分别进行排序,根据平面内的特征靶标构成的三角形在投影前后其面积大小排序不变的原理对平面内的特征靶标的投影进行匹配。
16.根据权利要求13所述的一种基于视觉的装备姿态位置测量系统,其特征在于,方法三为:将上一时刻4个平面内特征靶标的投影中心坐标与当前时刻正常提取的3个平面内特征靶标的投影中心坐标分别计算相对距离,共12组数据,利用相对距离最小的3组数据完成当前时刻正常提取投影与上一时刻特征靶标投影的对应关系,完成特征靶标投影的匹配,确定故障投影对应的特征靶标序号。
17.根据权利要求13所述的一种基于视觉的装备姿态位置测量系统,其特征在于,位姿解算子系统进行装备姿态位置的解算方法包括:
步骤1,设置景深dn的初始值所述景深dn为特征靶标Pc,n距离成像平面的距离,其中Pc,n=[xc,n,yc,n,zc,n]T为特征靶标在图像获取设备坐标系Oc-XcYcZc中的坐标,n=1,2,...N,N为特征靶标的个数;
步骤2,计算Pc,n,其中投影正常提取的特征靶标的Pc,n的计算方法为:Pc,n k=dn kvn,式中,k表示当前迭代次数,其中,f为图像获取设备的焦距,xi,n,yi,n为特征靶标投影的中心在图像平面坐标系Oi-XiYi中的坐标,且Pi,n=[xi,n,yi,n]T;
投影提取故障的平面内特征靶标的Pc,n的计算方法为:
式中,下标g表示投影提取故障的平面内特征靶标,下标a、b和e表示投影提取正常的平面内特征靶标;
步骤3,根据Pc,n和Pt,n计算旋转矩阵R和位移矢量T,其中,Pt,n=[xt,n,yt,n,zt,n]T为特征靶标在装备体坐标系Ot-XtYtZt中的坐标,R和T分别表示装备体坐标系相对于图像获取设备坐标系的旋转和平移,即满足Pc,n=RPt,n+T;
步骤4,计算误差其中,
步骤5,对景深dn进行迭代,其中,投影正常提取的特征靶标的迭代方法为:投影提取故障的平面内特征靶标的迭代方法为:
步骤6,若两次迭代误差之差小于设定的阀值则停止迭代,输出旋转矩阵R和位移矢量T,并计算当前时刻提取失败的平面内特征靶标投影的理论投影坐标。
18.根据权利要求17所述的一种基于视觉的装备姿态位置测量系统,其特征在于,步骤3中位移矢量T的计算方法为:
19.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至9中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810341530.0A CN108827300A (zh) | 2018-04-17 | 2018-04-17 | 一种基于视觉的装备姿态位置测量方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810341530.0A CN108827300A (zh) | 2018-04-17 | 2018-04-17 | 一种基于视觉的装备姿态位置测量方法及系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN108827300A true CN108827300A (zh) | 2018-11-16 |
Family
ID=64154585
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810341530.0A Pending CN108827300A (zh) | 2018-04-17 | 2018-04-17 | 一种基于视觉的装备姿态位置测量方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108827300A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111508235A (zh) * | 2019-07-11 | 2020-08-07 | 杭州海康威视系统技术有限公司 | 无效车辆的监控方法、装置、设备及存储介质 |
CN113865617A (zh) * | 2021-08-30 | 2021-12-31 | 中国人民解放军火箭军工程大学 | 飞行器机动发射主动段后视景象匹配精确位姿修正方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103259962A (zh) * | 2013-04-17 | 2013-08-21 | 深圳市捷顺科技实业股份有限公司 | 一种目标追踪方法和相关装置 |
CN104154918A (zh) * | 2014-07-14 | 2014-11-19 | 南京航空航天大学 | 单目视觉导航特征点丢失故障处理方法 |
CN104504691A (zh) * | 2014-12-15 | 2015-04-08 | 大连理工大学 | 基于低秩纹理的摄像机位置和姿态测量方法 |
CN106197395A (zh) * | 2016-03-23 | 2016-12-07 | 南京航空航天大学 | 一种基于ni‑cvs的气浮台位置姿态确定方法 |
-
2018
- 2018-04-17 CN CN201810341530.0A patent/CN108827300A/zh active Pending
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103259962A (zh) * | 2013-04-17 | 2013-08-21 | 深圳市捷顺科技实业股份有限公司 | 一种目标追踪方法和相关装置 |
CN104154918A (zh) * | 2014-07-14 | 2014-11-19 | 南京航空航天大学 | 单目视觉导航特征点丢失故障处理方法 |
CN104504691A (zh) * | 2014-12-15 | 2015-04-08 | 大连理工大学 | 基于低秩纹理的摄像机位置和姿态测量方法 |
CN106197395A (zh) * | 2016-03-23 | 2016-12-07 | 南京航空航天大学 | 一种基于ni‑cvs的气浮台位置姿态确定方法 |
Non-Patent Citations (1)
Title |
---|
江春: "面向航天器在轨服务的鲁棒单目视觉导航方法研究", 《中国优秀硕士学位论文全文数据库•工程科技Ⅱ辑》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111508235A (zh) * | 2019-07-11 | 2020-08-07 | 杭州海康威视系统技术有限公司 | 无效车辆的监控方法、装置、设备及存储介质 |
CN113865617A (zh) * | 2021-08-30 | 2021-12-31 | 中国人民解放军火箭军工程大学 | 飞行器机动发射主动段后视景象匹配精确位姿修正方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9355453B2 (en) | Three-dimensional measurement apparatus, model generation apparatus, processing method thereof, and non-transitory computer-readable storage medium | |
CN106408609B (zh) | 一种基于双目视觉的并联机构末端运动位姿检测方法 | |
CN107810522B (zh) | 实时、基于模型的对象检测及姿态估计 | |
CN112925223B (zh) | 基于视觉传感网络的无人机三维跟踪虚拟测试仿真系统 | |
US6795590B1 (en) | SAR and FLIR image registration method | |
WO2019011958A1 (en) | INVARIANT FACE ALIGNMENT SYSTEM AND METHOD FOR INSTALLATION | |
Wang et al. | Point cloud and visual feature-based tracking method for an augmented reality-aided mechanical assembly system | |
CN106530358A (zh) | 仅用两幅场景图像标定ptz摄像机的方法 | |
Ling et al. | High-precision online markerless stereo extrinsic calibration | |
JP2012042396A (ja) | 位置姿勢計測装置、位置姿勢計測方法、およびプログラム | |
CN112053447A (zh) | 一种增强现实三维注册方法及装置 | |
Buch et al. | Prediction of ICP pose uncertainties using Monte Carlo simulation with synthetic depth images | |
JP2017036970A (ja) | 情報処理装置、情報処理方法、プログラム | |
CN111413698A (zh) | 一种面向水下机器人搜寻探摸的目标定位方法 | |
CN112652003A (zh) | 一种基于ransac测度优化的三维点云配准方法 | |
CN109443200A (zh) | 一种全局视觉坐标系和机械臂坐标系的映射方法及装置 | |
Cui et al. | An improved pose estimation method based on projection vector with noise error uncertainty | |
CN108827300A (zh) | 一种基于视觉的装备姿态位置测量方法及系统 | |
CN113814987B (zh) | 多相机机器人手眼标定方法、装置、电子设备及存储介质 | |
CN114581632A (zh) | 基于增强现实技术的零件装配误差检测方法、设备和装置 | |
CN104484647B (zh) | 一种高分辨率遥感图像云高度检测方法 | |
Aranda et al. | Omnidirectional visual homing using the 1D trifocal tensor | |
Wan et al. | A performance comparison of feature detectors for planetary rover mapping and localization | |
CN114742141A (zh) | 基于icp点云的多源信息数据融合研判方法 | |
Savii | Camera calibration using compound genetic-simplex algorithm |
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 | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Address after: 621000 Mianyang city of Sichuan Province Branch Chong Park Jiuhua Road No. 6 Applicant after: SICHUAN JIUZHOU ELECTRIC GROUP Co.,Ltd. Address before: 621000 6 Jiuhua Road, kchuang Park, Anzhou District, Mianyang, Sichuan Applicant before: SICHUAN JIUZHOU ELECTRIC GROUP Co.,Ltd. |
|
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181116 |