CN101539405B - 基于姿态传感器的多视角测量数据自拼合方法 - Google Patents

Abstract

本发明公开了一种基于姿态传感器的多视角测量数据自拼合方法，属于三维传感和测量领域。该方法将姿态传感器固定在测量设备上，在多次测量中保持被测物***置不动，姿态传感器与测量设备位置相对不变。先利用标定物标定出姿态传感器与测量设备各自坐标系间的旋转变换矩阵，根据姿态传感器自动获取自身旋转变换矩阵，计算出每次测量时测量设备坐标系所发生的旋转变换，并对每次测量数据进行相应的旋转变换，最后根据重叠区域处数据点法矢方向相同原理，采用聚类方法计算不同视角下测量数据的平移变换，实现多视角测量数据自拼合。本方法使用设备结构简单，成本低，灵活可靠，易实现。

Description

基于姿态传感器的多视角测量数据自拼合方法 

技术领域

本发明涉及一种在三维轮廓测量***，尤其涉及一种在三维轮廓测量***中增加姿态传感器实现多视角测量数据自拼合的方法，属于三维传感和测量领域。 

背景技术

三维轮廓数据的测量在航空航天、车辆船舶、机械制造、生物医学、纺织服装、工业设计等领域有着广泛的应用需求。基于机械、光学、声学、电磁学原理的各种测量方法均取得了巨大进展，尤其是以激光扫描法、干涉法和结构光方法为代表的光学非接触测量方法在许多领域中实现了商业应用。然而，各种三维轮廓测量方法一次测量都只能获得有限区域的表面数据，大型物体或者全方位轮廓的测量要通过移动测量设备或者移动被测物体进行多次测量(称为多视角测量)，这样不同次测量得到的数据位于不同的坐标系下，而不同坐标系下的测量数据拼合是一个公认的棘手问题。 

目前解决这一问题主要有三种方法：一是在被测物体表面黏贴球形或者圆形的人工标记点，通过两次测量间至少三个以上的共同标记点，实现两个测量数据集之间的坐标变换。这种方法增加了测量前的准备工作，而且黏贴标记点的地方模型本身的测量数据会缺失，对于小型的和细节丰富的物体这个问题尤其突出；另一种方法是通过机械机构控制不同次测量中测量设备与被测物体之间的相对运动，由机械机构的运动参数直接计算测量点集间的坐标变换关系，例如，把被测物体固定在数控旋转工作台上，或者把测量***安装在复杂的多关节机械手臂上。采用这些数控机械装置不但大幅增加测量成本，而且使得整个测量***的灵活性和对被测物体尺寸大小的适应性明显降低；第三种方法单纯通过后续处理算法对不同坐标系下输出的三维测量数据进行拼合，主要问题在于如何从点云数据中提取稳定可靠的特征，这种方法受不同被测物体的表面形状影响很大，不具有通用性，可靠性也较差。 

广泛应用于航空导航、跟踪控制、平台稳定领域中的姿态传感器，可以实时输出自身运动的姿态角度和角速度，具有性能稳定，精度高等特点，并且小型的姿态传感器体积仅有火柴盒大小，重量仅有几十克，十分易于安装使用。因此如果能够将结构小巧的姿态传感器应用于三维轮廓测量***中，使***在改变位置和姿态对被测物体进行多视角测量时，能够自行确定其自身的坐标变换，从而无需黏贴标记点，也无需旋转台、机械臂等数控机械装置，就可以稳定可靠地解决多视角测量的数据拼合问题，无疑是一个非常好的途径。但由于结构小巧的姿态角度传感器不能感知自身的平移量，而且姿态角度传感器坐标系具有自身的坐标系，因此要实现这一构想，需要解决姿态传感器坐标系与测量***坐标系之间的关系标定、旋转关系确定下的测量数据集之间平移向量求解等问题。目前尚没发现有相关方法公开。 

发明内容

本发明为提高三维轮廓数据测量的灵活性和稳定性而提出一种基于姿态传感器的多视角测量数据自拼合方法。 

一种基于姿态传感器的多视角测量数据自拼合方法，通过在三维测量设备上固定一个可以实时获取自身坐标系姿态变化参数的姿态传感器来实施，具体包括如下步骤： 

A.对测量设备坐标系与姿态传感器坐标系之间的旋转变换矩阵R_C进行标定； 

B.对被测物体进行首次测量，获得测量设备坐标系下的被测物体表面测量区域内的三维数据点坐标，并取该次测量时的测量设备坐标系为全局坐标系，同时记录该次测量时姿态传感器的姿态参数； 

C.保持被测物体不动，改变测量设备的位置和姿态从不同视角对被测物体进行测量：每次测量时，均根据姿态传感器自动捕获的自身姿态参数计算出相对于首次测量时姿态传感器的旋转变换矩阵R_M，并由该相对于首次测量时姿态传感器的旋转变换矩阵R_M和步骤A中得到的旋转变换矩阵R_C计算出测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L，利用该相对于全局坐标系的该旋转变换矩阵R_L对测量数据点坐标进行旋转变换； 

D.确定首次测量后的每次测量中测量设备坐标系相对于全局坐标系的平移变换向量，并利用所得到的平移变换向量对每次的测量数据点坐标进行平移变换； 

E.在经过步骤C和步骤D将多视角测量数据变换到全局坐标系的基础上，利用迭代方法进行每次测量数据坐标拼合的整体优化。 

本发明在测量***中增加姿态传感器，无需在被测物体表面和测量场景中布置标记点，也无需采用专门的数控机械装置，大大简化测量准备工作，增加测量的灵活性；不同视角下测量设备坐标系与全局坐标系的旋转变换关系通过姿态传感器实时获取，而平移变换的确定采用基于聚类的统计方法，这相对于单纯通过后续处理算法提取特征点进行多视角三维测量数据拼合的方法具有更高的稳定性，拼合速度也更快；本方法能够对旋转面等基于特征提取方法无法实现自拼合的被测物体进行多视角测量数据自拼合。 

附图说明

图1是本发明的方法流程图。 

图2是本发明优选实例中的姿态传感器与双目立体传感器测量示意图。 

具体实施方式

如图1所示，一种基于姿态传感器的多视角测量数据自拼合方法，通过在三维测量设备上固定一个可以实时获取自身坐标系姿态变化参数的姿态传感器来实施，包括如下步骤： 

A.对测量设备坐标系与姿态传感器坐标系之间的旋转变换矩阵R_C进行标定； 

B.对被测物体进行首次测量，获得测量设备坐标系下的被测物体表面测量区域内的三维数据点坐标，并取该次测量时的测量设备坐标系为全局坐标系，同时记录该次测量时姿态传感器的姿态参数； 

C.保持被测物体不动，改变测量设备的位置和姿态从不同视角对被测物体进行测量：每次测量时，均根据姿态传感器自动捕获的自身姿态参数计算出相对于首次测量时姿态传感器的旋转变换矩阵R_M，并由该相对于首次测量时姿态传感器的旋转变换矩阵R_M和步骤A中得到的旋转变换矩阵R_C计算出测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L，利用该相对于全局坐标系的该旋转变换矩阵R_L对测量数据点坐标进行旋转变换； 

D.确定首次测量后的每次测量中测量设备坐标系相对于全局坐标系的平移变换向量，并利用所得到的平移变换向量对每次的测量数据点坐标进行平移变换； 

E.在经过步骤C和步骤D将多视角测量数据变换到全局坐标系的基础上，利用迭代方法进行每次测量数据坐标拼合的整体优化。 

以上是本发明方法的总体步骤，现以双目立体测量***与姿态传感器为实施装置，并结合附图详述实施步骤： 

用φ(e，θ)表示旋转运动，其中：e为旋转轴的正向，θ∈[0，π]为绕旋转轴正向按右手法则旋转的角度，则刚体的旋转运动φ(e，θ)可以唯一确定旋转轴e所在坐标系的旋转变换矩阵R，其确定关系为： 

$R=\left[\begin{array}{ccc}{e}_x^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& e_x{e}_y(1-\cos \mathrm{\θ})-e_z\sin \mathrm{\θ}& e_x{e}_z(1-\cos \mathrm{\θ})+e_y\sin \mathrm{\θ}\\ e_x{e}_y(1-\cos \mathrm{\θ})+e_z\sin \mathrm{\θ}& e_y^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}& e_y{e}_z(1-\cos \mathrm{\θ})-e_x\sin \mathrm{\θ}\\ e_x{e}_z(1-\cos \mathrm{\θ})-e_y\sin \mathrm{\θ}& e_y{e}_z(1-\cos \mathrm{\θ})+e_x\sin \mathrm{\θ}& e_z^2(1-\cos \mathrm{\θ})+\cos \mathrm{\θ}\end{array}\right]---\left(1\right)$

其中：e_x、e_y、e_z分别为旋转轴正向e的三个分量。 

反之，若已知坐标系旋转变换矩阵R＝{r_ij}，根据旋转矩阵的性质，知道旋转矩阵必有三个特征值，分别为1，cosθ±isinθ，其中特征值1所对应的特征向量为旋转轴的方向(同向或反向)，θ∈[0，π]为绕旋转轴正向的旋转角度，因此成立下面的关系： 

1+2cosθ＝tr(R)，θ∈[0，π]    (2) 

(R-I)e＝0                       (3) 

当R≠I时，(3)式的解为一维解空间，设单位解向量e＝(e_x，e_y，e_z)^T。根据(1)式比较矩阵R关于对角线对称的矩阵元素大小，可得下面的事实： 

$\left\{\begin{array}{ccc}{e}_z\≥0& \mathrm{if}& r_{21}\≥r_{12}\\ e_y\≤0& \mathrm{if}& r_{31}\≥r_{13}\\ e_x\≥0& \mathrm{if}& r_{32}\≥r_{23}\end{array}\right.---\left(4\right)$

结合(4)式的关系，即可确定由(3)式求解得到的旋转轴的正向单位向量，旋转角度θ由(2)式唯一确定。 

本实施例将姿态传感器固定在双目立体测量设备上，并始终保持姿态传感器与双目立体测量设备相对位置不变，如图2所示，当双目立体测量设备做刚体运动时，姿态传感器也做相同的刚体运动。设姿态传感器坐标系为O_M，双目立体测量设备坐标系为O_L，O_M与O_L坐标系之间存在着刚体变换，记该刚体变换中旋转矩阵为R_C，平移向量为T_C，则有 

P_M＝R_CP_L+T_C    (5) 

其中：P_L为被测物体上的点在O_L下的坐标，P_M为该点在O_M下的坐标。 

对测量设备坐标系与姿态传感器坐标系之间的旋转变换关系进行标定的具体方法是：先对标定物进行首次测量，然后移动双目立体测量设备从不同视角对标定物进行第二次测量，两次测量之间测量设备整体的位置和姿态变化可以视为刚体运动，令φ(e，θ)表示该刚体运动中的旋转运动，则φ(e，θ)在姿态传感器坐标系和测量设备坐标系下所确定的旋转角度一定相同，而旋转轴正向不同。设第二次标定测量的坐标系O_L和O_M相对于第一次标定测量分别存在旋转矩阵R_L和R_M。为计算旋转矩阵R_L，需要在这两次标定测量中至少测量出标定物上三个不共线的相同数据点，不妨设P_i和P_i′(i＝1，2，...，k)分别表示第一次和第二次测量得到的标定物上相同数据点在测量设备坐标系中的坐标，并记： 

$\stackrel{\‾}{P}=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{P}_i,$ ${\stackrel{\‾}{P}}^{\′}=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{P_i}^{\′},$ Q_i＝P_i-P，Q_i′＝P_i′P′ 

构造矩阵 $W=\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{Q}_i{Q_i}^{\′T},$ 对W进行奇异值分解得到W＝UDV^T，其中：U、V为正交矩阵，D为对角矩阵，则有 

R_L＝VU^T    (6) 

矩阵R_M是由姿态传感器自动反馈的绕其自身坐标系三个坐标轴的旋转角度转化得到。根据(3)(4)可以分别确定出φ(e，θ)在两个不同坐标系下的旋转轴正向单位向量为e_L和e_M，则e_L和e_M之间必然存在旋转变换，即： 

R_Ce_L＝e_M        (7) 

要确定旋转矩阵R_C，至少需要三组上述方程(7)，因此要求在标定过程中，至少需要从不同视角对标定物进行四次或四次以上的测量。假设经过n+1(n≥3)次测量，得到n个方程： 

R_Ce_Li＝e_Mi，i＝1，2，3，...，n    (8) 

对(8)式采用奇异值分解方法进行最小二乘求解。记矩阵 $W^{\′}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{e}_{\mathrm{Li}}{e_{\mathrm{Mi}}}^T,$ 对W′进行奇异值分解得到W′＝U′D′V′^T，其中U′、V′为正交矩阵，D′为对角矩阵，则(8)式的最小二乘解为： 

R_C＝V′U′^T        (9) 

根据本发明的测量设备坐标系与姿态传感器坐标系之间的旋转变换关系标定方法，只需要对标定物进行四次或四次以上不同视角的标定测量，并且仅要求根据标定测量结果可以计算出每次标定测量时测量设备所产生的旋转变换矩阵即可，操作简单，易于实现。 

在对被测物体测量过程中，将被测物体固定不动，改变测量设备的位置和姿态，进行多视角测量。取对被测物体进行首次测量时的测量设备坐标系作为全局坐标系。第二次测量时，由姿态传感器实时捕获本次测量时姿态传感器自身坐标系O_M相对于全局坐标系的旋转变换矩阵R_M，根据R_M通过(2)(3)(4)式即可求得φ(e_M，θ_M)，再根据e_L＝R_C ^Te_M和θ_L＝θ_M，可以确定本次测量的测量设备坐标系O_L相对于全局坐标系的旋转运动φ(e_L，θ_L)。将e_L，θ_L代入(1)式中即可求得测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L。 

用上述旋转矩阵R_L对第二次测量所得的三维点坐标实施旋转变换，使得第二次测量时测量设备坐标系与全局坐标系仅相差一个平移变换。为了求解该平移变换，要求第二次测量数据和全局坐标系中的测量数据之间必须有部分数据为重叠数据，即被测物体上有部分数据在至少两个视角下的测量中同时出现。由于此时重叠区域三维点坐标仅相差一个平移变换，因此第二次测量数据和全局坐标系中测量数据中对应重叠区域内的数据点其法矢方向是一致的，根据这个原理，本发明利用聚类方法求解未知的平移向量，具体方法如下： 

设已经处于全局坐标系中的测量数据点集和经过旋转变换后的第二次测量数据点集分别记为{P_i}和{Q_j}，在本实施例中将{P_i}和{Q_j}分别进行三角化并估计每个顶点处的法矢，与数据点集相对应，记法矢分别为{NP_i}和{NQ_j}。如果将每个点处单位法矢的三个分量看作一个点的坐标，则这些点必然在以原点为球心，半径为1的球面上。由于姿态传感器自身误差和测量数据重叠区域的数据不完全重合以及测量***自身误差等原因，实施例中判断法矢NP_i和NQ_j为相同，仅当NP_i和NQ_j满足下面不等式： 

NP_i·NQ_j＞cos1⁰        (10) 

对{P_i}中每个点P_s的法矢NP_s，在{Q_j}中搜索得到Q_t，使得NP_s·NQ_t最大，并且满足(10)式，记P_s和Q_t之间的平移向量为： 

T_st＝P_s-Q_t    (11) 

则T_st为一个可能的平移向量。利用(10)和(11)式计算出{P_i}和{Q_j}之间所有可能的平移向量，并将这些平移向量的起点移至坐标原点，再以每个平移向量的终点为球心，做半径为r的球(实施例中取r＝0.5)，统计落入该球内其它平移向量终点的个数，得到包含最多平移向量终点所对应的球，则取该球的球心为这第二次测量时测量设备坐标系与全局坐标系之间的平移变换向量。 

利用所求得的平移向量，对第二次测量数据点集{Q_j}进行平移变换，即可将第二次测量时测量设备坐标***一到全局坐标系中。第三次以及后续每次测量都采用与第二次测量相同的方法将测量数据变换到全局坐标系下。最后利用迭代方法(如迭代最近点(ICP)方法) 对所有点云数据进行全局拼合优化，进一步提高多视角测量数据的自拼合精度。 

可以理解对于本领域的技术人员来说，在本发明方法基础上可能存在多种的改进类型。因而上述描述和相关的附图只是本发明的示意而不局限于此。 

Claims (4)

1.一种基于姿态传感器的多视角测量数据自拼合方法，其特征在于：通过在三维测量设备上固定一个可以实时获取自身坐标系姿态变化参数的姿态传感器来实施，具体包括如下步骤：

A.对测量设备坐标系与姿态传感器坐标系之间的旋转变换矩阵R_C进行标定；

B.对被测物体进行首次测量，获得测量设备坐标系下的被测物体表面测量区域内的三维数据点坐标，并取该次测量时的测量设备坐标系为全局坐标系，同时记录该次测量时姿态传感器的姿态参数；

C.保持被测物体不动，改变测量设备的位置和姿态从不同视角对被测物体进行测量：每次测量时，均根据姿态传感器自动捕获的自身姿态参数计算出相对于首次测量时姿态传感器的旋转变换矩阵R_M，并由该相对于首次测量时姿态传感器的旋转变换矩阵R_M和步骤A中得到的旋转变换矩阵R_C计算出测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L，利用该相对于全局坐标系的该旋转变换矩阵R_L对测量数据点坐标进行旋转变换；

D.确定首次测量后的每次测量中测量设备坐标系相对于全局坐标系的平移变换向量，并利用所得到的平移变换向量对每次的测量数据点坐标进行平移变换；

E.在经过步骤C和步骤D将多视角测量数据变换到全局坐标系的基础上，利用迭代方法进行每次测量数据坐标拼合的整体优化。

2.根据权利要求1所述的基于姿态传感器的多视角测量数据自拼合方法，其特征在于：所述步骤A中对测量设备坐标系与姿态传感器坐标系之间的旋转变换矩阵R_C进行标定的方法包括如下步骤：

F.标定物保持不动，移动测量设备从至少四个不同视角对标定物进行测量；

G.根据每次对标定物进行测量的结果，计算在每次测量时测量设备坐标系所产生的旋转变换矩阵，同时获取该次测量时姿态传感器所反馈的姿态传感器坐标系所产生的旋转变换矩阵；

H.根据步骤G中得到的每次测量设备坐标系的旋转变换矩阵，计算每次测量设备刚体运动的旋转轴正向单位向量和旋转角度，同时根据步骤G中得到的每次姿态传感器坐标系产生的旋转变换矩阵，计算每次姿态传感器刚体运动的旋转轴正向单位向量和旋转角度；

I.根据每次测量时测量设备坐标系旋转变换的旋转轴正向单位向量与姿态传感器坐标系旋转变换的旋转轴正向单位向量之间相差同一个旋转变换的约束条件，求解测量设备坐标系与姿态传感器坐标系之间的旋转变换矩阵。

3.根据权利要求1所述的基于姿态传感器的多视角测量数据自拼合方法，其特征在于：所述步骤C中计算测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L的方法包括如下步骤：

J.根据步骤C中相对于首次测量时姿态传感器的旋转变换矩阵R_M计算出旋转运动φ(e_M，θ_M)，其中：e_M-旋转轴正向单位向量，θ_M-旋转角度；

K.计算测量设备的旋转运动φ(e_L，θ_L)，并由该旋转运动φ(e_L，θ_L)确定测量设备坐标系相对于全局坐标系的旋转变换矩阵R_L，其中：旋转轴正向单位向量e_L＝R_C ^Te_M，旋转角度θ_L＝θ_M；

4.根据权利要求1所述的基于姿态传感器的多视角测量数据自拼合方法，其特征在于：所述步骤D中确定首次测量后的每次测量中测量设备坐标系相对于全局坐标系的平移变换向量的方法包括如下步骤：

L.在已经位于全局坐标系中的三维数据点和已经经过步骤C中相对于全局坐标系的旋转变换矩阵R_L旋转变换的数据点中，查找法矢相同的对应点，并分别计算每对对应点之间的平移向量； 

M.对步骤L中得到的所有平移向量进行聚类分析，将包含平移向量个数最多的那一类的中心点所对应的平移向量作为所要确定的测量设备坐标系相对于全局坐标系的平移变换向量。 

Images