专利名称：一种空间目标三维位姿视觉测量方法
技术领域：
本发明涉及测量技术，具体说就是一种空间目标三维位姿视觉测量方法。
(二)
背景技术：
三维位姿测量是指求解两个坐标系间的相对位姿关系，包括三个平移量和分别绕三个坐标轴的旋转量。基于点特征的单目视觉位
姿测量方法通过单相机对目标成像，利用目标像中3个以上非共线特征点的像坐标求解目标与相机间的相对位置和相对姿态，由于其结构简单、易于实现等特点成为视觉测量领域研究热点之一，并广泛应用于汽车、机器人、飞机、航天器等领域的非接触式测量任务。目前，基于点特征的单目视觉位姿测量算法大致可分为解析算法和迭代算法两类，解析算法在特征点数较少时能够推导出目标相对位姿参数的解析解，具有运算量小、计算速度快等优点，然而由
于在实际应用成像过程中的测量误差、量化误差、特征点提取误差或特征点匹配错误等因素影响，使用解析算法可能会产生很大的误差，并且解析算法仅适用于特征点数小于6的情况。迭代算法是利用透视成像的基本原理，将相对位姿确定问题表示为受约束的非线性优化问题，通过求解该优化问题得到目标相对位姿的数值解，能够有效提高存在误差情况下的相对位姿参数测量精度。
DeMenthon等提出利用弱透视像机模型，基于特征点对应的迭代算法，当使用Newton递推算法时，迭代算法能够以超线性速度局部收敛。然而，这些数值方法的性能取决于初始值选取，且算法通常会收敛到局部最小值，或者收敛到一个错误解。为解决该问题Lee等推导了约束流形上的Gauss-Newton迭代算法，把相对位姿确定问题描述为 一个在旋转矩阵流形和为确保正的深度参数所决定矩阵约束锥的交空间上三个旋转参数最优化问题，并且提出了在
Gauss-Newton方向和随机方向之间变化的解析测地线搜索，以确保不重新初始化算法就能跳出局部最小，收敛至全局最小。然而上述基于优化方法的相对位姿测量算法是一个优化变量空间为7V+6维(7V为点特征数)的非线性问题，计算量较大，难以满足工业领域应用的实时性要求。
Haralick等提出了 一种基于点特征同时计算目标位姿和特征点景深的位姿确定算法，该算法通过引入特征点景深变量消除了由于透视投影所产生位姿确定问题的非线性，且具有全局收敛性，然而该算法局部收敛速度较慢。
发明内容
本发明的目的在于提供一种具有精度高、收敛快、计算量小、适用范围广、三维位姿计算过程采用两解析算法迭代进行求解的空间目标三维位姿^L觉测量方法。
本发明的目的是这样实现的采用在被测空间目标上设置特征光标点的视觉测量方式，包括视觉相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算。所述的三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程，且绝对定向问题解算和景深估计两个阶段均为解析方式求解；其过程为
(1) 初始化各特征光标的景深值《),,'=1,2,...,^,利用像点坐标计算相应的逆投影线单位矢量u, = (V(X,2 + ]f + (《K /)'，
置迭代计数器/^l;
(2) 利用各特征点景深值《"，重构各特征点坐标《("=《 ,，并
采用Umeyama算法等解析算法求解绝对定位问题得到空间
目标的三维位姿参数似t')，《^;
(3)计算4 = / -，以及= J 2^, —=、 +《正步骤(2)给出三维位姿为=Mf)/"')=c;)+r("
4(4) 更新景深值《阔=";(M"+1、
(5) 计算误差
e2(M("",,("",{C=力cf+1、-+,("")|2;
如果误差小于预定误差阈值五，，即,
则进入第(7)步，否则，置yt一A + l;
(6) 若迭代次数少于预定的最大迭代次数，返回到第(2)步，否则进入第(7)步；
(7) 迭代结束，输出结果。
本发明一种空间目标三维位姿视觉测量方法，所述的三维位姿计算采用两解析算法迭代进行的求解方式，具有精度高、收敛快、计算量小、适用范围广等优点，具体体现在
(1) 采用迭代方式求解空间目标三维位姿，有效降低了测量过程中各类误差的影响；
(2) 迭代求解的两阶段都是解析方式求解，降低了计算复杂度，并具有良好的收敛性；
点的构型信息，因此能够适用于3个以上不同特征点数、各种非共线构型的测量特征光标的空间目标三维位姿测量。
(四)
图1为本发明的三维位姿计算方法流程框图；图2为本发明所涉及到的坐标系，图中分别定义了相机参考坐标系、像平面坐标系和目标参考坐标系；
图3为本发明物空间内物像共线误差示意图；图4为本发明实施例的测量特征光标点设置示意图；图5为本发明实施例的各特征点景深运行结果曲线；图6为本发明实施例的空间目标相对位置运行结果曲线；图7为本发明实施例的空间目标相对姿态运行结果曲线。
(五)
具体实施例方式
5下面结合附图举例对本发明作进一步说明。
实施例l，结合图1、图3,本发明空间目标三维位姿视觉测量方法是基于逆投影线思想，引入特征点的景深变量，将三维位姿计算过程分解为迭代进行的绝对定向解算和景深估计两阶段。在绝对定向解算阶段釆用绝对定向解算解析算法计算空间目标的相对姿态
旋转矩阵和相对平移矢量；在景深估计阶段利用绝对定向解算阶段给出的相对位姿重构各特征点物玄间坐标,并用重构的特征点物32间坐标在逆投影线上的投影更新各特征点的景深。上述两阶段迭代进行直至结果收敛。
所述的三维位姿计算中，每个特征光标像点都对应一条从相机投影中心出发的射线，该射线穿过特征光标像点指向空间目标，由
于该射线方向与特征光标点的投影线相反，故称为逆投影线，其单4立矢量u,可表示为
丄
理想情况下特征像点{s,:(《,。}和特征点{s::(《,"',z:w在逆投影
线",上，称之为共线条件，因此特征点《:在相机坐标系中可表示为' "1,2,…，W (2)
其中《为目标特征点距离投影中心的景深。
共线条件也可表述为特征点《在逆投影线",上的投影等于其自
身，因此共线方程也可表示为
<formula>formula see original document page 6</formula>(3)
对比式(2)和式(3)可知，目标特征点《的景深《可近似表示为
<formula>formula see original document page 6</formula>(4)
由于图像处理过程中存在误差，目标特征点通常不在由特征像点所重构的逆投影线上，由式(2)表示的特征点坐标《与实际特征点坐标
有一定误差，可以利用该误差定义目标函数
<formula>formula see original document page 6</formula> (5)基于点特征的空间目标三维位姿视觉测量问题可描述为给定 特征光标点在目标坐标系中的物点坐标^ :(x,，乂,z,),hl,2，…,W ，及其 相应的像点坐标{《(1,，]0,/ = 1,2,...,^},求解旋转矩阵M、平移矢量, 和景深{《,/ = 1,2,...，〃},使得式(5)表示的目标函数取最小值。
在引入逆投影线和景深变量后，空间目标三维位姿计算问题可 分解为两个问题其一，计算各特征点的景深^,/ = 1,2，...,]^,进而
由式(2)重构各特征点的三维物空间坐标《；其二，利用特征点三维 重构坐标《和3维测量特征模型坐标&,计算测量特征相对相机的 相对姿态和相对位置，进而得到空间目标三位位姿。
上述两个问题的求解分别称为景深估计和绝对定向问题，因此， 本发明给出了 一种基于逆投影思想的包括景深估计和绝对定向解算 两阶段的空间目标三维位姿计算方法，在景深恢复阶段计算各特征 点的景深^,,,' = 1,2,...,7^,在绝对定向解算阶段利用特征点三维重构 坐标《和3维模型坐标s,,并基于绝对定向解算解析算法计算出空
间目标三维位姿，上述两阶段迭代进行直至结果收敛。 1、绝对定向解算阶段
该阶段是利用特征点三维重构坐标《和三维模型坐标s,求解两 个三维模型所在坐标系之间的平移和旋转参数，即绝对定向结算问
题。目前，针对该问题有多种解析算法，包括Faugeras和Horn分别 提出的基于四元数的解析算法，Arun等提出的基于旋转矩阵特征值 分解的解析算法，以及Umeyama在Amn算法基础上的修正算法等。 由于Umeyama所提出的基于特征值分解的修正算法在具有测量误 差时的鲁棒性，以及运算量小等优点，本发明选择该算法进行绝对 方位解算，具体的Umeyama算法参见S.Umeyama等的论文 "Least-squares estimation of transformation parameters between two point patterns"(S. Umeyama. Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence. 1991, 13(4):367陽380)。采用解析算法求解能够显著有效降低算法计算量，提高算法的 运算速度和收敛速度。
2、景深估计阶段
在由Umeyama算法给出空间目标相对位置和相对姿态Mt",《:" 之后，将式(4)表示的景深近似算法修正为
(+')";0c+c')+r阔) (6)
其中，r(""为第^+l次迭代的平移矢量修正量。
在景深更新算法修正为式(6)后，式(5)表示的目标函数可表示为
(7)
对式(7)相对7^+')求偏导并令其为零得:
a 2 Af阔"+') T
1 ^
=0
(4,4+1)
(8)
=> r阔=士zk、-v c)]
将式(6)代入式(8)得
r阔=碑"化v《+e"+严"》,-K:>,+e")] 4,(《'vC -(《've)]+谅""')"，
,=〗 ,=，
由式(9)可得
w一;^,",'))r(阔=仏'_/K=VC")
(10)
其中/为3阶单位方阵。
由于tv/-!^,d是正定矩阵，其逆存在，由式(10)可得平移矢量
增量r一为
i;( ,')i;(",",'K，,+,
(11)
第壯l次迭代的相对位姿参数M"+"，,""为M—Mume (12) 式(6)给出的景深更新算法变为
+,("") (13)
进行上述修正后能够有效利用特征点物空间坐标的第壯i次迭 代值，由其在逆投影线上的投影长度精确近似第壯i次景深。
理论证明本发明所提出的算法具有全局收敛性，该方法流程框 图如图i所示。
实施例2，本发明空间目标三维位姿视觉测量方法，还具有以下 技术特征
(1) 所述的绝对定向解算算法是一种绝对定向问题求解的解析 算法，如Umeyama解析算法等；
(2) 所述的绝对定向解算阶段还包括对Umeyama解析算法给 出的三维位姿的修正，即般(""=7^=)， /("""H)+r(""，
其中
r阔=询/-";)〕
本发明的三维位姿测量方法包含如下步骤
(1) 初始化各特征光标的景深值《(°),,' = 1,2,...,^,利用像点坐标计 算相应的逆投影线单位矢量",=Q + lf+/2))"(《X /)'，
置迭代计数器& = 1; .
(2) 利用各特征点景深值()，重构各特征点坐标《("=《 ,，并
采用Umeyama解析算法求解绝对定位问题得到空间目标的 三维位姿参数Mt",《:"；
(3) 计算」,=/-"x,并计算"""=-fi>,ViM(o,+e)，
、,y乂 /=1
修正步骤(2)给出三维位姿为=^C+e')，,阔=《:e" + ;
(4) 更新景深值《""
(5) 计算误差
9如果误差小于预定误差阈值i^ ，即Z(M("V阔,，
则进入第(7)步，否则，置&:=& + 1;
(6) 若迭代次数少于预定的最大迭代次数，返回到第(2)步，否 则进入第(7)步；
(7) 迭代结束，输出结果。 (
本发明的三维位姿计算采用两解析算法迭代进行的求解方式， 具有精度高、收敛快、计算量小、适用范围广等优点，具体体现在
(1) 采用迭代方式求解空间目标三维位姿，有效降低了测量过程 中各类误差的影响；
(2) 迭代求解的两阶段都是解析求解，降低了计算复杂度，并具 有良好的收敛性；
点的构型信息，因此能够适用于3个以上不同特征点数、各 种非共线构型的测量特征光标的空间目标三维位姿测量。 实施例3，结合图4，根据本发明提出的空间目标三维位姿视觉 测量方法，建立了实际测量系统，光标点设置如图4所示，光标点 & : (x,,乂，z,)7',, = 1,…，4在目标坐标系中的坐标分别为:
0X2_0.20.2x40
A :y，=-0.2，少2=0，& :0，& :0.2
05.00Z40
空间目标相对测量相机的相对姿态为
= [10°,15°,8°]，相对 位置为,=[4,-1,6]';相机焦距0.051m;像平面大小19.2mmx 14.4mm; 像平面分辨率1600x1200;像元大小12(^mxl2pm;特征点提耳又 误差1像素(3cj)。利用本发明的运行结果如图5-图7所示，从图中 可以看出在较大初值偏差情况下，所提出的三维位姿计算方法在30 次迭代内能够收敛到正确值，验证了其全局收敛性。相对姿态角测 量精度优于0.5度，相对位置测量精度优于2cm，能够满足实际应用 的测量精度要求。
10
权利要求
1. 一种空间目标三维位姿视觉测量方法，基于单目视觉的空间目标三维位姿测量方法，采用在被测空间目标上设置特征光标点的视觉测量方式，包括视觉相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算过程，其特征在于所述的三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程。
2. 根据权利要求1所述的三维位姿计算，其特征在于绝 对定向问题解算和景深估计两个阶段均为解析方式求解；其过程 为(1) 初始化各特征光标的景深值《、、1,2，…,W，利用^f象点坐标计 算相应的逆投影线单位矢量= Q(X,2 + )f + /2(X, ^ /)'，置迭代计数器/t-i;(2) 利用各特征点景深值^)，重构各特征点坐标""=《 ,，并采用Umeyama解析算法求解绝对定位问题得到空间目标的三维位姿参数《，,《:"(3) 计算4=/-",";,以及^+1)=-1>, 修V '=1 乂 ,=i正步骤(2)给出三维位姿为m("" = iw^)/"') = + r("";(4) 更新景深值《+"(5) 计算误差e2(M阔/一,^阿卜力l(+'、 -(M("、 +"+I)f ;如果误差小于预定误差阈值五，，即e2—("v阔,w"i〈五，，则进入第(7)步，否则，置&:=* + 1;(6) 若迭代次数少于预定的最大迭代次数，返回到第(2)步，否则 进入第(7)步；(7) 迭代结束，输出结果。
全文摘要
本发明提供一种三维位姿计算采用绝对定向问题解算和景深估计两阶段迭代求解的空间目标三维位姿视觉测量方法。本发明采用在被测目标上设置特征光标点的视觉测量方式，包括相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算。三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程在绝对定向解算阶段采用绝对定向解算解析算法计算空间目标的相对位姿；在景深估计阶段利用前一阶段给出的相对位姿重构各特征点物空间坐标，并用其在逆投影线上的投影更新各特征点的景深。三维位姿计算采用两解析算法迭代进行的求解方式，具有精度高、收敛快、计算量小、适用范围广等优点。
文档编号G01B11/00GK101464134SQ20091007129
公开日2009年6月24日 申请日期2009年1月16日 优先权日2009年1月16日
发明者凡 张, 张世杰, 徐国栋, 曹喜滨, 晖 李 申请人:哈尔滨工业大学