CN105678088A - 一种靶标测头的平差优化算法 - Google Patents

Abstract

本发明公开了一种靶标测头的平差优化算法：通过寻找向量在欧氏空间与摄影空间的不变量关系作为约束条件，即建立在不同位姿下手持靶标靶面特征点B到靶标测头A的空间向量及其在针孔相机模型下的成像规律，以该约束条件作为条件方程建立函数模型，采取Helmet验后方差估计作为随机模型，最后将差值带入条件方程，完成差值检验。本算法充分利用了每一个手持靶标靶面特征点的空间位置信息，鲁棒性好，稳定性高，提高了手持靶标测头的3D空间还原精度，为进一步提高双目立体视觉测量精度提供了有效的方法。

Description

一种靶标测头的平差优化算法

【技术领域】

本发明属于视觉测量技术领域，是一种可以提高双目立体视觉系统测量精度，用于还原手持靶标测头空间3D坐标的一种平差优化算法。

【背景技术】

随着各种异形工件内腔外形检测需求的增多，以立体视觉测量为基础的便携式3D坐标测量技术得到越来越多的应用。双目立体视觉系统只需要两台摄像机、标定板、手持靶标和专用测头，就能完成对大型复杂型腔零件的在位/在线测量，不受现场环境影响，可靠性高、尤其是对复杂内腔、深长孔等测量有独特的优势。

在双目立体视觉系统中，手持靶标是一个重要的测量设备。手持靶标作为双目立体视觉系统中一个重要的坐标传递设备，是连接手持靶标LED特征点空间3D坐标和测头3D坐标的媒介。测量时，手持靶标的测头与待测物体表面接触，标定完成后的摄像机通过捕捉手持靶标靶面特征点，完成特征点的3D空间坐标还原，利用建立的靶标坐标系完成从靶面特征点的3D空间坐标到测头坐标转换，从而最终测得与测头接触的待测物体表面3D空间坐标。因此，从手持靶标靶面得到LED特征点的3D空间坐标后，如何建立靶标坐标系用来还原待测点，即手持靶标测头的空间3D坐标，是空间定位三维重建的一个难题。

目前，建立靶标测头坐标系的方法主要依靠靶标自标定技术，即将手持靶标的测头与一个标定块锥形内槽底部接触，标定块固定，转动手持靶标放置若干不同位姿，相当于靶标测头不动，靶面LED的特征点一直在变换空间3D坐标，利用靶面LED特征点与靶标测头相对位置不变的关系，列出方程求解测头与每一个LED特征点的距离，从而以某3个不共线的LED特征点建立的坐标系作为靶标坐标系时，就可以找出此坐标系下测头的坐标，还原出空间中测头的3D坐标，也就可以得到与测头接触的目标待测点的坐标。

然而，利用自标定技术建立靶标坐标系存在以下问题：

首先，如何设计靶面LED特征点的数目。要建立起一个完整的靶标坐标系，至少需要3个不共线的特征点，特征点数目较少的时候由于受偏心提取误差、镜头畸变等原因误差影响会相对较大；然而增加特征点的数目，利用LED特征点到靶标测头距离不变的条件所列出的超定方程的系数矩阵是一个病态矩阵，条件数大，受误差影响大，难以得到理想的解。

其次，每一次还原待测点的空间3D坐标时，利用有效的LED特征点的数目较少。在得出靶面每个LED特征点到测头的距离以后，只能利用其中的3个不共线特征点进行靶标坐标系的建立，如果靶面一共布置N个LED特征点，则一共可以建立个靶标坐标系(假设没有任意共线的3个LED特征点)，利用靶标坐标系作为坐标传递的媒介可以得到个待测点的空间3D坐标，将这些3D坐标平均求和就可以得到最终的待测点空间3D坐标，虽然可以利用平均值减小误差，但是这些数据的方差相对较大。

最后，由于使用的是待测点的空间3D坐标的均值，每个数据在最终结果中所占的权重没有区别，每个数据的可靠程度无法有效的度量，因此有可能将误差放大。

以上问题正在越来越多的被国内外学者关注，目前很少有关此类问题的国内外相关文献，也没有完整的相对成熟的解决方法。因此，针对还原靶标测头坐标的问题，本发明提出了一种鲁棒的靶标测头的平差优化算法。

【发明内容】

本发明提出了一种靶标测头的平差优化算法，以提高手持靶标特征点计算靶标测头坐标的精度，即提高与测头接触的待测点的空间3D坐标精度。

本发明采用以下技术方案：

一种靶标测头平差优化算法，首先建立空间向量与其在CCD成像之间的关系，然后构建条件平差模型计算平差值，最后对该平差值进行验证；其中，构建条件平差模型的过程中，在确定了观测数之后，假定初始的各个参数的权值均为1，在此假定条件下计算平差值，如果最终的平差值未通过验证，则重新调整各个参数的权值重新计算直至通过验证。

空间向量与其在CCD成像之间的关系为：

$\mathrm{\Δ}u=\frac{\sqrt{f_R^2+d_{oef}^2}\·d\·{\sin }^2{\mathrm{\θ}}_1}{d_T\cos ({\mathrm{\θ}}_1-\mathrm{\α})\[\sin ({\mathrm{\θ}}_1+\mathrm{\β})\cos \mathrm{\α}+\frac{d}{d_T}{\mathrm{cos\αsin\θ}}_1-{\mathrm{sin\θ}}_1\cos (\mathrm{\α}+\mathrm{\β})\]}$

其中，是有效焦距，f_R是右相机的实际焦距，d_oef是主点与BAO_R平面的距离，d是向量的模长，θ₁是测头A与CCD摄像机光心o连线与向量的夹角，d_T是两个相机光心O_LO_R之间的距离，α是向量与CCD摄像机光轴的夹角，β是右相机光轴与两个相机光心连线的夹角。

构建条件平差模型计算平差值的具体方法包括以下步骤：

(1)以空间向量与其在CCD成像之间的关系为条件式；

(2)对步骤(1)的条件式进行线性化；

(3)确定权阵：在初始权阵P中，假定各个参数的权值均为1；

(4)确定改正数向量V：

(5)确定闭合差W：W＝A·(Δu,θ₁,d)^T，其中，

(6)确定随机模型并计算平差值。

在确定闭合差以后首先计算出改正数向量的一个初值，然后再确定随机模型计算平差值，计算改正数向量的方法包括以下步骤：

(A)将条件式改写为：

(B)构造联系数法方程：

(B1)利用拉格朗日条件极值构造乘系数向量K：

Φ＝V^TPV-2K^T(AV-W)

其中，

AV-W＝0

条件式为：V＝P^-1A^TK，

AV-W＝0

含K的改正数向量为：V＝P^-1A^TK

则，法方程为：AP^-1A^TK-W＝0；

(B2)令联系数法方程系数N为：N＝AP^-1A^T，

则乘系数向量K为：

K＝N^-1W

(B3)利用以下公式计算改正数向量：

V＝P^-1A^TK。

所述步骤(6)的具体步骤包括以下：

(6.1)将步骤(1)条件式中的所有观测值均用参数形式表示，具体如下：

${\tilde{L}}_1={\tilde{X}}_{{\mathrm{\θ}}_1}={\tilde{X}}_1$

${\tilde{L}}_2={\tilde{X}}_d={\tilde{X}}_2$

${\tilde{L}}_3={\tilde{X}}_{\mathrm{\Δ}u}=\frac{\∂f}{\∂{\mathrm{\θ}}_1}{\tilde{X}}_{{\mathrm{\θ}}_1}+\frac{\∂f}{\∂d}{\tilde{X}}_d=\frac{\∂f}{\∂{\mathrm{\θ}}_1}{\tilde{X}}_1+\frac{\∂f}{\∂d}{\tilde{X}}_2,$

令： $B=(\frac{\∂f}{\∂{\mathrm{\θ}}_1},\frac{\∂f}{\∂d}),$ 则闭合差为： $\underset{n\×1}{l}=\underset{n\×t}{L}-\underset{n\×t}{B}\underset{t\×1}{X^0}-\underset{n\×1}{d},$ 改正数向量为： $\underset{n\×1}{V}=\underset{n\×t}{B}\underset{t\×1}{x}-\underset{n\×1}{l},$

求解参数改正数向量x：x＝(B^TPB)^-1B^TPl，

则参数的修正值为：

观测值的修正值为：

利用Helmet方差分量估计严密式计算单位权方差，以对方差值进行验证，Helmet方差分量估计严密式为：

$\underset{2\×2}{S}\underset{2\×1}{\widehat{\mathrm{\θ}}}=W_{\mathrm{\θ}}$

其中， $S=\left[\begin{array}{cc}{n}_1-2tr\left(N_1{N}^{-1}\right)+tr\left(N_1{N}^{-1}{N}_1{N}^{-1}\right)& tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)\\ tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)& n_2-2tr\left(N_2{N}^{-1}\right)+tr\left(N_2{N}^{-1}{N}_2{N}^{-1}\right)\end{array}\right]$

$W_{\mathrm{\θ}}={\left[\begin{array}{cc}{V}_1^T{P}_1{V}_1& V_2^T{P}_2{V}_2\end{array}\right]}^T$

$\widehat{\mathrm{\θ}}={\left[\begin{array}{cc}{\widehat{\mathrm{\σ}}}_{01}^2& {\widehat{\mathrm{\σ}}}_{02}^2\end{array}\right]}^T$

N＝N₁+N₂， $N_1=B_1^T{P}_1{B}_1,N_2=B_2^T{P}_2{B}_2;$

若则认为所设定的权值合理。

重新调整权值采用以下公式：

$P_1=\frac{{\mathrm{\σ}}_0^2}{{\widehat{\mathrm{\σ}}}_{01}^2},P_2=\frac{{\mathrm{\σ}}_0^2}{{\widehat{\mathrm{\σ}}}_{02}^2}.$

与现有技术相比，本发明至少具有以下有益效果：本发明首先通过寻找向量在欧氏空间与摄影空间的不变量关系作为约束条件，即建立在不同位姿下手持靶标靶面特征点B到靶标测头A的空间向量及其在针孔相机模型下的成像规律，以该约束条件作为条件方程建立函数模型，最后进行验证结果，完成差值检验。本发明方法充分利用了每一个手持靶标靶面特征点的空间位置信息，鲁棒性好，稳定性高，提高了手持靶标测头的3D空间还原精度，为进一步提高双目立体视觉测量精度提供了有效方法。

【附图说明】

图1为双目系统中空间向量在针孔模型中成像的示意图。

其中O_L为左相机光心，O_R为右相机光心，为空间向量，其中A点为手持靶标的测头，B为靶面上LED特征点，θ₁为与的夹角，θ₂为与的夹角。

图2为在双目视觉系统中取单相机，在简单放置条件下，针孔模型中向量成像的示意图。其中(A)、(B)分别为向量不穿过摄像机光轴与穿过摄像机光轴两种位置情况的示意图。

其中，O₁为相机的光心，CDEF所在平面为CCD成像平面，为空间向量，θ₁为与的夹角，θ₂为与的夹角。

图3为图2局部，(A)、(B)分别为向量不穿过摄像机光轴与穿过摄像机光轴两种位置情况的示意图。

其中，α为相机光轴与向量夹角所称的锐角，EF为向量在CCD相面所称的像，θ₁为与的夹角，θ₂为与的夹角。

图4为向量在空间一般位置情况下，在针孔相机模型下成像的示意图。

其中O_R为相机光心，图中的虚线表示，过相机光心与向量平行的直线。

图5为图4的局部，表示在ABO_R平面内向量成像的示意图。

图6为验后方差分量估计算法流程图。

图7为靶标测头平差优化算法流程图。

【具体实施方式】

本发明算法公开了两种方案，以下分别介绍：

方案一：

本算法是在使用以手持靶标的双目立体视觉系统测量空间定位时的优化算法，旨在提高以手持靶标靶面特征点作为空间特征点还原靶标测头时的精度，目的是提高双目立体视觉系统的测量精度。其具体实施操作存在于双目立体视觉系统测量过程。下面结合某一次测量过程对本发明作进一步详细说明。(本次测量使用的手持靶标以LED作为空间的特征点)

第一步：标定。

首先需要利用张正友棋盘格标定算法对搭建好的双目立体视觉系统进行标定，得到标定参数。

第二步：获取图像信息。

标定完成以后，利用手持靶标对待测的表面进行逐点测量。即移动手持靶标至需要测量的目标点，然后两个相机同时拍照，捕捉到手持靶标在左右两个不同位姿的相机的图像。虽然获取的是图像，但其有效信息是图像中手持靶标靶面上的LED空间特征点的像。本方案利用加权重心算法，将LED在2维平面的像点面(大约100-200个像素单位左右)拟合成一个亚像素的像素坐标。

第三步：匹配。

匹配的目的是将同一个LED在左右两个相机不同的像联系起来，形成一一对应的关系。能否正确的匹配决定了特征点三维还原的正确与否。由于LED在靶面分布的特殊性，本方案利用基于极径极角的匹配算法，其运算速度快，鲁棒性极好，保证了匹配的正确定。

第四步：三维还原。

1、LED光心偏差方向的确定

两个关键方向与能够确定偏差方向。其中，

(1)LED发光面的法线方向

(2)摄像机光心到LED发光中心的向量

LED光心偏差方向根据以下公式计算确定：

λ₁,λ₂分别代表校正方向在二维坐标系下的两个分量。

2、粗提取

利用加权重心算法对左右摄像机捕捉的手持靶标LED特征点灰度值图像进行光心坐标粗提取。

左相机光心坐标右相机光心坐标其中i＝1,2,3,...n表示特征点个数。

3、降噪声

降噪的对象是左右相机内外参标定结果构造出来的基础矩阵F₀。由于噪声、提取误差和随机误差的存在，利用标定结果构造出来的基础矩阵F₀并不满足双目立体视觉中基础矩阵rank(F₀)＝2的性质，这为后面最佳校正位置的判断带来影响，所以对其进行降噪。

其中，利用左右相机内外参标定结果构造出来的基础矩阵F₀为：

F₀＝K'^-TSRK^-T

其中K'右相机的内参，K是左相机的内参，R是外参的旋转矩阵，S是外参的平移矩阵T的反对称矩阵。

将F₀进行奇异值分解

F₀＝Udiag(λ₁,λ₂,λ₃)V^T

对最小的奇异值操作λ₃＝0得到

F＝Udiag(λ₁,λ₂,0)V^T

4、粗定位

粗定位分为三步。

首先，将步骤2粗提取的左右图像平面光心坐标沿着步骤1中求得的各个点的校正方向依次移动步长P_j＝ε₁*j(0.01pixel≤ε₁≤0.05pixel)，(ε₁为每次移动的步长，j为一共移动的步数)，左或右图像平面所有点移动一个步长ε形成的图像称之为点位图，则左右图像平面可以形成j*j个点位图对。

然后，对粗提取出来的图像光心坐标进行归一化坐标变换，计算每一个点位图对对应的基础矩阵解除归一化变换得到原始点(原始点指未移动之前的坐标位置)对应基础矩阵F_i。

最后，将与j*j个点位图对相对应的j²个基础矩阵写成列向量的形式构造出一个9行j²列的矩阵M₁，利用主成分分析(PCA)算法对M₁进行特征空间重构，取前6个特征值及其对

应的特征向量，构成矩阵M′₁。将矩阵M′₁中所有列向量进行样本投影重构，将每一个基础矩阵(列向量)与均值向量的差值投影到特征空间。寻找样本空间中与目标欧氏距离最短、夹角最小的基础矩阵。

空间向量与目标欧式距离按照以下公式计算：

$d_i^2=\left|\right|f_i-f_1|{|}^2$

其中，f₁为目标向量，f_i是第i个空间向量。

空间向量与目标向量的夹角θ_i按照以下公式计算：

${\mathrm{cos\&theta;}}_i=\frac{<f_i\&CenterDot;f_1>}{\left|\right|f_i\left|\right|*\left|\right|f_1\left|\right|}$

以此时筛选出的基础矩阵对应点位图对的左右图像平面光心坐标位置为粗定位的结果。

5、精定位

将单步步长变为ε₂，ε₂＝ε₁/10。其中，校正方向以粗定位结果为中心，沿着步骤1得到的校正方向直线，左右各移动n/2个步长ε₂，则可以得到n*n个点位图对，也就是对应有n²个基础矩阵，重复步骤4，此时筛选出的基础矩阵对应点位图对的左右图像平面光心坐标位置为最终校正结果。

在获取精确的靶面LED特征点的3D空间位置坐标以后，就可以利用靶标测头平差优化算法还原测头的空间位置坐标。

6、利用约束条件建立空间向量在欧氏空间与摄影空间的关系，即建立空间向量(靶面LED特征点到测头)与其在CCD成像之间联系。

通过之前的推导可知，向量在CCD摄像机相面成像长度Δu与其模长d的关系可以表示为：

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

是修正以后的有效焦距，f_R是右相机的实际焦距，d_oef是主点(图像平面的中点)与BAO_R平面的距离，d是向量的模长，d_T是两个相机光心O_LO_R之间的距离，β是右相机光轴与两个相机光心连线的夹角(取锐角值)。

7、构建函数模型

这里使用的函数模型是条件平差模型

1)确定必要的观测数

这里我们使用的条件方程就是在1中推导的约束条件：

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

方程中有8个未知数d,θ₁,Δu和f_R,d_oef,d_T,α,β，其中f_R,d_oef,d_T,α,β可以通过标定得到，那么此条件方程得到d,θ₁,Δu之间的关系。

2)确定条件方程并定权

A确定条件式

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

B将条件式线性化

Δu＝f(θ₁,d)取一阶偏导，得到

$\widetilde{\mathrm{\&Delta;}u}-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\widetilde{\mathrm{\&theta;}}}_1-\frac{\&part;f}{\&part;d}\tilde{d}=0$

C确定权阵

初始权阵P中，各个参数权值都设为1

P＝(P_d,P_θ1,P_Δu)＝(1,1,1)

P_d,P_θ1,P_Δu分别为三个参数权值。

D确定改正数向量

用V表示改正数向量，则有：

$V={(v_{\mathrm{\&Delta;u}},v_{{\mathrm{\&theta;}}_1},v_d)}^T$

其中v_Δu表示 $V={(v_{\mathrm{\&Delta;u}},v_{{\mathrm{\&theta;}}_1},v_d)}^T,$

那么含有改正数向量的线性化方程可以表示为：

$(\mathrm{\&Delta;}u+v_{\mathrm{\&Delta;}u})-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}({\mathrm{\&theta;}}_1+v_{{\mathrm{\&theta;}}_1})-\frac{\&part;f}{\&part;d}(d+v_d)=0$

E确定闭合差

闭合差可以表示为：

$W=\mathrm{\&Delta;}u-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\mathrm{\&theta;}}_1-\frac{\&part;f}{\&part;d}d$

写成矩阵的形式：

W＝A·(Δu,θ₁,d)^T

其中 $A=(1,-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1},-\frac{\&part;f}{\&part;d})$

8、确定随机模型

这里使用的随机模型是验后方差分量估计，又称为赫尔默特方差分量估计。

平差前观测值向量的方差阵一般是未知的，因此平差时随机模型都是使用观测值向量的权阵。而权的确定往往都是采用经验定权，也称为随机模型的验前估计，对于同类观测值可按第一章介绍的常用定权方法定权；对于不同类的观测值，就很难合理地确定各类观测值的权。为了合理地确定不同类观测值的权，可以根据验前估计权进行预平差，用平差后得到的观测值改正数来估计观测值的方差，根据方差的估计值重新进行定权，以改善第一次平差时权的初始值，再依据重新确定的观测值的权再次进行平差，如此重复，直到不同类观测值的权趋于合理。

1)将平差方程写成含参数的形式

将条件方程Δu＝f(θ₁,d)中的所有观测值均用参数形式表示：

${\tilde{L}}_1={\tilde{X}}_{{\mathrm{\&theta;}}_1}={\tilde{X}}_1$

${\tilde{L}}_2={\tilde{X}}_d={\tilde{X}}_2$

${\tilde{L}}_3={\tilde{X}}_{\mathrm{\&Delta;}u}=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_{{\mathrm{\&theta;}}_1}+\frac{\&part;f}{\&part;d}{\tilde{X}}_d=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_1+\frac{\&part;f}{\&part;d}{\tilde{X}}_2$

其中B为系数矩阵:

$B=(\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1},\frac{\&part;f}{\&part;d})$

则闭合差可以表示为：

$\underset{n\&times;1}{l}=\underset{n\&times;t}{L}-\underset{n\&times;t}{B}\underset{t\&times;1}{X^0}-\underset{n\&times;1}{d}$

改正数向量可以表示为：

$\underset{n\&times;1}{V}=\underset{n\&times;t}{B}\underset{t\&times;1}{x}-\underset{n\&times;1}{l}$

求解参数改正值向量x：

x＝(B^TPB)^-1B^TPl

则参数的修正值为：

$\hat{X}=X+x$

观测值的修正值为：

$\hat{L}=L+V$

2)利用Helmet方差分量估计严密式计算单位权方差

Helmet方差分量估计严密式为：

$\underset{2\&times;2}{S}\underset{2\&times;1}{\widehat{\mathrm{\&theta;}}}=W_{\mathrm{\&theta;}}$

其中：

$S=\left[\begin{array}{cc}{n}_1-2tr\left(N_1{N}^{-1}\right)+tr\left(N_1{N}^{-1}{N}_1{N}^{-1}\right)& tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)\\ tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)& n_2-2tr\left(N_2{N}^{-1}\right)+tr\left(N_2{N}^{-1}{N}_2{N}^{-1}\right)\end{array}\right]$

$W_{\mathrm{\&theta;}}={\left[\begin{array}{cc}{V}_1^T{P}_1{V}_1& V_2^T{P}_2{V}_2\end{array}\right]}^T$

$\widehat{\mathrm{\&theta;}}={\left[\begin{array}{cc}{\widehat{\mathrm{\&sigma;}}}_{01}^2& {\widehat{\mathrm{\&sigma;}}}_{02}^2\end{array}\right]}^T$

N＝N₁+N₂， $N_1=B_1^T{P}_1{B}_1,N_2=B_2^T{P}_2{B}_2$

n₁、n₂分别为第一类、第二类平差量的数目，B₁、B₂分别为第一类、第二类平差量的系数矩阵，P₁、P₂分别为第一类、第二类平差量权值矩阵，分别为第一类、第二类平差量单位权方差，V₁、V₂分别为第一类、第二类平差量该整数向量。第一类为角度，第二类为长度(即距离)。

3)判断此次的权值是否合理

利用之前所确定的权值计算出来的单位权方差进行判断，若则认为所设定的权值是合理的，否则需要对权值进行调整。

4)对权值进行调整

若单位权方差值比相差较大，需要重新定权：

$P_1=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{01}^2},P_2=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{02}^2}$

重新定权以后按照Helmet方差分量估计严密式判断权值是否合理。

9、进行差值检验

将最终定权以后的差值结果回带条件方程中，看是否满足条件方程的要求。

方案二：

方案二与方案一的区别仅在于：利用靶标测头平差优化算法还原测头的空间位置坐标，具体地说，就是方案一的步骤6至步骤9，更具体地说，其区别在于：在确定随机模型之前，改正数向量V是提前计算出一个初值，还是不用计算，直接用随机模型计算。

因此，方案二仅针对平差优化算法进行阐述。

本发明提出的靶标测头平差优化算法主要包括以下几个步骤：

1、寻找约束条件

建立空间向量(靶面LED特征点到测头)与其在CCD成像之间联系。

1)简单放置下，针孔相机模型的空间向量成像规律，参见图1。

将手持靶标靶面LED特征点B到靶标测头点A相连接，就可以构建一个空间向量其在CCD相面成像为为了简化，首先假设此向量位于与通过CCD摄像机光轴的平面内。α是向量与CCD摄像机光轴的夹角，θ₁是测头A与CCD摄像机光心o连线与向量的夹角，θ₂是靶面LED特征点B与CCD摄像机光心o连线与向量的夹角，通过三角形正弦余弦定理消去θ₂。

以右相机为例，向量在CCD摄像机相面成像长度Δu与其模长d的关系可以表示为：

$\mathrm{\&Delta;}u=\frac{f_R\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})cos\mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

f_R是右相机的实际焦距，d是向量的模长，d_T是两个相机光心O_LO_R之间的距离，β右相机光轴与两个相机光心连线的夹角(取锐角值)。

2)将简单放置推广到一般测量条件下，空间向量的成像规律。

简单放置条件是将空间向量限制于通过CCD摄像机光轴的平面内，一般条件下，空间向量与CCD摄像机光轴会有夹角，但是，总可以将摄像机光心与空间向量相连接形成一个三角形，如图4所示。再将其实际焦距f_R转化成在此平面内的有效焦距即可，如图5所示。

以右相机为例，一般条件下，向量在CCD摄像机相面成像长度Δu与其模长d的关系可以表示为：

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

是修正以后的有效焦距，f_R是右相机的实际焦距，d_oef是主点(图像平面的中点)与BAO_R平面的距离，d是向量的模长，d_T是两个相机光心O_LO_R之间的距离，β是右相机光轴与两个相机光心连线的夹角(取锐角值)。

2、构建函数模型

这里使用的函数模型是条件平差模型

1)确定必要的观测数

这里我们使用的条件方程就是在1中推导的约束条件：

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

方程中有8个未知数d,θ₁,Δu和f_R,d_oef,d_T,α,β，其中f_R,d_oef,d_T,α,β可以通过标定得到，那么此条件方程得到d,θ₁,Δu之间的关系。

2)确定条件方程并定权

A确定条件式

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

B将条件式线性化

Δu＝f(θ₁,d)取一阶偏导，得到

$\widetilde{\mathrm{\&Delta;}u}-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\widetilde{\mathrm{\&theta;}}}_1-\frac{\&part;f}{\&part;d}\tilde{d}=0$

C确定权阵

初始权阵中，各个参数权值都设为1

P＝(P_d,P_θ1,P_Δu)＝(1,1,1)

D确定改正数向量

用V表示改正数向量，则有：

$V={(v_{\mathrm{\&Delta;u}},v_{{\mathrm{\&theta;}}_1},v_d)}^T$

其中v_Δu表示 $V={(v_{\mathrm{\&Delta;u}},v_{{\mathrm{\&theta;}}_1},v_d)}^T,$

那么含有改正数向量的线性化方程可以表示为：

$(\mathrm{\&Delta;}u+v_{\mathrm{\&Delta;}u})-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}({\mathrm{\&theta;}}_1+v_{{\mathrm{\&theta;}}_1})-\frac{\&part;f}{\&part;d}(d+v_d)=0$

E确定闭合差

闭合差可以表示为：

$W=\mathrm{\&Delta;}u-\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\mathrm{\&theta;}}_1-\frac{\&part;f}{\&part;d}d$

写成矩阵的形式：

W＝A·(Δu,θ₁,d)^T

其中 $A=(1,-\frac{\&PartialD;f}{\&PartialD;{\mathrm{\&theta;}}_1},-\frac{\&PartialD;f}{\&PartialD;d})$

D确定条件方程

条件方程可以表示为：

$\underset{r\&times;n}{A}\underset{n\&times;1}{V}-\underset{r\&times;1}{W}=\underset{r\&times;1}{0}$

3)构造法方程

未知数(改正数v)的个数比条件方程多，所以该方程的解不定，采用最小二乘法进行约束，利用拉格朗日条件极值构造乘系数向量K，

Φ＝V^TPV-2K^T(AV-W)

其中条件极值和含K的改正向量分别表示为：

AV-W＝0

V＝P^-1A^TK

那么联系数法方程可以表示为：

AP^-1A^TK-W＝0

4)求解联系数

由以上联系数法方程可知联系数法方程系数N：

N＝AP^-1A^T

则K可以表示为：

K＝N^-1W

5)解算改正数

带入联系数法方程系数K就可以得到改正数向量：

V＝P^-1A^TK

6)计算平差值

带入原数据向量，得到平差后的观测数值

$\hat{L}=L+V$

3、确定随机模型

这里使用的随机模型是验后方差分量估计，又称为赫尔默特方差分量估计。

平差前观测值向量的方差阵一般是未知的，因此平差时随机模型都是使用观测值向量的权阵。而权的确定往往都是采用经验定权，也称为随机模型的验前估计，对于同类观测值可按第一章介绍的常用定权方法定权；对于不同类的观测值，就很难合理地确定各类观测值的权。为了合理地确定不同类观测值的权，可以根据验前估计权进行预平差，用平差后得到的观测值改正数来估计观测值的方差，根据方差的估计值重新进行定权，以改善第一次平差时权的初始值，再依据重新确定的观测值的权再次进行平差，如此重复，直到不同类观测值的权趋于合理。

1)将平差方程写成含参数的形式

将条件方程Δu＝f(θ₁,d)中的所有观测值均用参数形式表示：

${\tilde{L}}_1={\tilde{X}}_{{\mathrm{\&theta;}}_1}={\tilde{X}}_1$

${\tilde{L}}_2={\tilde{X}}_d={\tilde{X}}_2$

${\tilde{L}}_3={\tilde{X}}_{\mathrm{\&Delta;}u}=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_{{\mathrm{\&theta;}}_1}+\frac{\&part;f}{\&part;d}{\tilde{X}}_d=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_1+\frac{\&part;f}{\&part;d}{\tilde{X}}_2$

其中B为系数矩阵:

$B=(\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1},\frac{\&part;f}{\&part;d})$

则闭合差可以表示为：

$\underset{n\&times;1}{l}=\underset{n\&times;t}{L}-\underset{n\&times;t}{B}\underset{t\&times;1}{X^0}-\underset{n\&times;1}{d}$

改正数向量可以表示为：

$\underset{n\&times;1}{V}=\underset{n\&times;t}{B}\underset{t\&times;1}{x}-\underset{n\&times;1}{l}$

求解参数改正值向量x：

x＝(B^TPB)^-1B^TPl

则参数的修正值为：

$\hat{X}=X+x$

观测值的修正值为：

$\hat{L}=L+V$

2)利用Helmet方差分量估计严密式计算单位权方差

Helmet方差分量估计严密式为：

$\underset{2\&times;2}{S}\underset{2\&times;1}{\widehat{\mathrm{\&theta;}}}=W_{\mathrm{\&theta;}}$

其中：

$S=\left[\begin{array}{cc}{n}_1-2tr\left(N_1{N}^{-1}\right)+tr\left(N_1{N}^{-1}{N}_1{N}^{-1}\right)& tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)\\ tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)& n_2-2tr\left(N_2{N}^{-1}\right)+tr\left(N_2{N}^{-1}{N}_2{N}^{-1}\right)\end{array}\right]$

$W_{\mathrm{\&theta;}}={\left[\begin{array}{cc}{V}_1^T{P}_1{V}_1& V_2^T{P}_2{V}_2\end{array}\right]}^T$

$\widehat{\mathrm{\&theta;}}={\left[\begin{array}{cc}{\widehat{\mathrm{\&sigma;}}}_{01}^2& {\widehat{\mathrm{\&sigma;}}}_{02}^2\end{array}\right]}^T$

N＝N₁+N₂， $N_1=B_1^T{P}_1{B}_1,N_2=B_2^T{P}_2{B}_2$

n₁、n₂分别为第一类、第二类平差量的数目，B₁、B₂分别为第一类、第二类平差量的系数矩阵，P₁、P₂分别为第一类、第二类平差量权值矩阵，分别为第一类、第二类平差量单位权方差，V₁、V₂分别为第一类、第二类平差量该整数向量。第一类为角度，第二类为长度(即距离)。

3)判断此次的权值是否合理

利用之前所确定的权值计算出来的单位权方差进行判断，若则认为所设定的权值是合理的，否则需要对权值进行调整。

4)对权值进行调整

若单位权方差值比相差较大，需要重新定权：

$P_1=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{01}^2},P_2=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{02}^2}$

重新定权以后按照Helmet方差分量估计严密式判断权值是否合理。

4、进行差值检验

将最终定权以后的差值结果回带条件方程中，看是否满足条件方程的要求。

Claims (7)

1.一种靶标测头平差优化算法，其特征在于：首先建立空间向量与其在CCD成像之间的关系，然后构建条件平差模型计算平差值，最后对该平差值进行验证；其中，构建条件平差模型的过程中，在确定了观测数之后，假定初始的各个参数的权值均为1，在此假定条件下计算平差值，如果最终的平差值未通过验证，则重新调整各个参数的权值重新计算直至通过验证。

2.根据权利要求1所述的一种靶标测头平差优化算法，其特征在于：空间向量与其在CCD成像之间的关系为：

$\mathrm{\&Delta;}u=\frac{\sqrt{f_R^2+d_{oef}^2}\&CenterDot;d\&CenterDot;{\sin }^2{\mathrm{\&theta;}}_1}{d_{T}cos({\mathrm{\&theta;}}_1-\mathrm{\&alpha;})\&lsqb;sin({\mathrm{\&theta;}}_1+\mathrm{\&beta;})\cos \mathrm{\&alpha;}+\frac{d}{d_T}{\mathrm{cos\&alpha;sin\&theta;}}_1-{\mathrm{sin\&theta;}}_{1}cos(\mathrm{\&alpha;}+\mathrm{\&beta;})\&rsqb;}$

其中，是有效焦距，f_R是右相机的实际焦距，d_oef是主点与BAO_R平面的距离，d是向量的模长，θ₁是测头A与CCD摄像机光心o连线与向量的夹角，d_T是两个相机光心O_LO_R之间的距离，α是向量与CCD摄像机光轴的夹角，β是右相机光轴与两个相机光心连线的夹角。

3.根据权利要求2所述的一种靶标测头平差优化算法，其特征在于：构建条件平差模型计算平差值的具体方法包括以下步骤：

(1)以空间向量与其在CCD成像之间的关系为条件式；

(2)对步骤(1)的条件式进行线性化；

(3)确定权阵：在初始权阵P中，假定各个参数的权值均为1；

(4)确定改正数向量V：

(5)确定闭合差W：W＝A·(Δu,θ₁,d)^T，其中，

(6)确定随机模型并计算平差值。

4.根据权利要求3所述的一种靶标测头平差优化算法，其特征在于：在确定闭合差以后首先计算出改正数向量的一个初值，然后再确定随机模型计算平差值，计算改正数向量的方法包括以下步骤：

(A)将条件式改写为：

(B)构造联系数法方程：

(B1)利用拉格朗日条件极值构造乘系数向量K：

Φ＝V^TPV-2K^T(AV-W)

其中，

AV-W＝0

条件式为：V＝P^-1A^TK，

AV-W＝0

含K的改正数向量为：V＝P^-1A^TK

则，法方程为：AP^-1A^TK-W＝0；

(B2)令联系数法方程系数N为：N＝AP^-1A^T，

则乘系数向量K为：

K＝N^-1W

(B3)利用以下公式计算改正数向量：

V＝P^-1A^TK。

5.根据权利要求3或4所述的一种靶标测头平差优化算法，其特征在于：所述步骤(6)的具体步骤包括以下：

(6.1)将步骤(1)条件式中的所有观测值均用参数形式表示，具体如下：

${\tilde{L}}_1={\tilde{X}}_{{\mathrm{\&theta;}}_1}={\tilde{X}}_1$

${\tilde{L}}_2={\tilde{X}}_d={\tilde{X}}_2$

${\tilde{L}}_3={\tilde{X}}_{\mathrm{\&Delta;}u}=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_{{\mathrm{\&theta;}}_1}+\frac{\&part;f}{\&part;d}{\tilde{X}}_d=\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1}{\tilde{X}}_1+\frac{\&part;f}{\&part;d}{\tilde{X}}_2,$

令： $B=(\frac{\&part;f}{\&part;{\mathrm{\&theta;}}_1},\frac{\&part;f}{\&part;d}),$ 则闭合差为： $\underset{n\&times;1}{l}=\underset{n\&times;t}{L}-\underset{n\&times;t}{B}{\underset{t\&times;1}{X}}^0-\underset{n\&times;1}{d},$ 改正数向量为： $\underset{n\&times;1}{V}=\underset{n\&times;t}{B}\underset{t\&times;1}{x}-\underset{n\&times;1}{l},$

求解参数改正数向量x：x＝(B^TPB)^-1B^TPl，

则参数的修正值为：

观测值的修正值为：

6.根据权利要求1所述的一种靶标测头平差优化算法，其特征在于：利用Helmet方差分量估计严密式计算单位权方差，以对方差值进行验证，Helmet方差分量估计严密式为：

$\underset{2\&times;2}{S}\underset{2\&times;1}{\widehat{\mathrm{\&theta;}}}=W_{\mathrm{\&theta;}}$

其中， $S=\left[\begin{array}{cc}{n}_1-2tr\left(N_1{N}^{-1}\right)+tr\left(N_1{N}^{-1}{N}_1{N}^{-1}\right)& tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)\\ tr\left(N_1{N}^{-1}{N}_2{N}^{-1}\right)& n_2-2tr\left(N_2{N}^{-1}\right)+tr\left(N_2{N}^{-1}{N}_2{N}^{-1}\right)\end{array}\right]$

$W_{\mathrm{\&theta;}}={\left[\begin{array}{cc}{V}_1^T{P}_1{V}_1& V_2^T{P}_2{V}_2\end{array}\right]}^T$

$\widehat{\mathrm{\&theta;}}={\left[\begin{array}{cc}{\widehat{\mathrm{\&sigma;}}}_{01}^2& {\widehat{\mathrm{\&sigma;}}}_{02}^2\end{array}\right]}^T$

$N=N_1+N_2,N_1=B_1^T{P}_1{B}_1,N_2=B_2^T{P}_2{B}_2;$

若则认为所设定的权值合理。

7.根据权利要求6所述的一种靶标测头平差优化算法，其特征在于：重新调整权值采用以下公式：

$P_1=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{01}^2},P_2=\frac{{\mathrm{\&sigma;}}_0^2}{{\widehat{\mathrm{\&sigma;}}}_{02}^2}.$