基于罗德里格矩阵的3D点云拼接的6DOF求解方法研究

基于罗德里格矩阵的3D点云拼接的6DOF求解方法研究

孙庆江1，黄新红1，付洪军1，邵家旺1

指导老师：刘科利，姚吉利

(1山东理工大学建筑工程学院)

摘要 在本文中，首先介绍了变换矩阵的概念，表达点云拼接参数，方便地计算计算点云坐标变换的6个自由度（six degrees of freedom，6DOF）。现有的6DOF解算方法中，最有效方法是ICP法，把3个平移量和3个旋转角为参数，其模型比较复杂，计算时间长。本文利用反对称矩阵和罗德里格矩阵的性质,把传统的三个旋转角参数用反对称矩阵的三个独立元素代替，推导了用三个公共点计算任意旋转角情况下的6个参数的直接计算公式，并建立了相应的平差模型，可以快速确定。

关键字　点云拼接，6DOF，ICP，3D坐标转换，罗德里格矩阵

1 引言

两块点云拼接需要确定6个自由度(Besl ,McKay, 1992)，快速有效的两块点云同名点寻找的算法是ICP(iterative closest point)法，6个自由度确定后，把一块点云坐标转换到另一个点云的坐标中，高精度的点云拼接由自由度的精度确定。

为了方便计算点云坐标变换的6个自由度，用下面一个4 × 4的坐标转换矩阵来存储矩阵R，偏移参数，缩放因子，旋转因子，如下：

式中为平移参数，是罗德里格矩阵中三个参数，缩放因子为1，其它9个元素构成旋转矩阵。

2 六参数数学模型

本文中使用的方法有以下优点：①旋转角度 使用罗德里格矩阵的反对称矩阵中的进行替换 ；② 准确直接地计算任何旋转矩阵的六参数的值③ 一个确定的平差模型。此方法没有使用传统的三角函数而是使用简单的加减乘除进行运算，减少了计算量和提高了运算速度。

根据坐标转换的物理过程,可得到数学模型:

各符号的意义参见参考文献[1]。, , ,通常被称作六参数

在绝对定向中，上式左边可认为表示摄影测量模型坐标系统下的坐标，右边表示地面坐标系统下坐标；为平移因子，其意义是参考点（坐标转换所选的一个公共点）旋转后的坐标；λ为尺度因子(六参数中其值为1)；R 为坐标转换旋转矩阵，，是把原坐标绕Z轴旋转角得到的旋转矩阵，是绕新的 X 轴旋转得到的旋转矩阵，是绕新 Y轴旋转得到的旋转矩阵。

所以

，，，，， 为6参数，后3个为旋转参数或角度参数。

旋转参数：

在任意条件下，3 个角取值范围是 0°~360°，具体大小无法判断。实际应用中，只要解出转换矩阵就能达到坐标转换的目的。

令

是一个正交矩阵,其 9 个元素中只有 3 个是独立的。 又设反对称矩阵:

其元素是独立的。是由构成的罗德里格矩阵。

3　6DOF求解

3.1 罗德里格矩阵的数学模型

3维直角坐标转换中，当在两坐标系统下有3个公共点时，就可惟一解算出6个转换参数；多余3个公共点时，就要进行平差计算，转换参数的初值(特别是旋转角)的大小，直接影响平差系统稳定性和计算速度，有时使得解算的参数均严重偏离其值。任意角度的3维坐标转换计算用传统的方法很难正确解算。用反对称矩阵和罗德里格矩阵性质推出的公式可以准确计算转换参数模型，适应于任意旋转角的坐标转换。该模型计算速度快，结果准确。

在一个方阵内，当对角线上个元素为零，而与主对角线相对称的各元素值相等且符号相反时，该矩阵称为反对称矩阵。例如：

其特点是。用单位阵I加反对称矩阵S与单位阵减反对称矩阵的逆矩阵的逆矩阵相乘也是一个正交矩阵。

反对称矩阵和罗德里格矩阵性质：

其中，I 是3阶单位阵，将矩阵R展开得：

其中：

经整理得：

该形式的旋转矩阵式罗德里格于1840年推倒而得到的，因而称罗德里格矩阵。需要指出的是罗德里格矩阵中三个参数a，b，c并不是方向余弦。

3.2 6DOF的确定

当两坐标系有 3 对公共点时，6 个参数惟一确定，可直接解算出来。由数学建模过程可以得出平移参数只有在旋转矩阵 R确定后才能确定，所以旋转矩阵的确定是参数解算的核心。R矩阵9 个元素是由 3 个角度确定的，所以只有 3 个是独立的。设反对称矩阵：

其中 a,b,c 是相互独立的。R由S构成罗德里格矩阵

公式(3)称为罗德里格矩阵。其中,

在本文中，, ,由a,b,c替换, 可以确定一组方程。

根据公式(1)，由公共点1，2可列1个方程组：

公式(4)仅包括两个独立参数无法求出3个未知量。对于1点和2点类似于公式 (4)可写出一组方程。

其中;;;;.然后，参数就可以通过下面的公式计算出来。

其中.旋转矩阵能够通过公式 公式.(3) 和公式公式..(1)进行计算和转换。.

3.3 6DOF解算平差模型

在实际测量中，为了检测误差和更高的精度，超过3个共同点可以得到一个最小二乘解。

， 计算出未知值的初始近似值，公式1可线性，线性表达式如下：

其中表示转换参数的矩阵

系数矩阵如下:

其中,.

4 实验与分析

在实验中，两个坐标系统的坐标是模拟的，这六个参数的真值已知。在两个坐标系统中模拟了共同点的130组坐标，每组20个同名点，计算的平移参数和罗德里格矩阵中三个参数a，b，c与真值之差列于表1。

表1 计算值与真值比较

从表可以看出当旋转角度很小(的值小)时，解出的6DOF值和真值之间差的很小。由130组坐标计算的平移参数和罗德里格矩阵中三个参数a，b，c与真值之差统计出X轴、Y轴、Z轴平移量中误差为4.5mm，的误差为。

图1显示了六个参数的误差分布，很明显，解出的基于罗德里格矩阵的6个参数值与他们的真值差的很小。

5 结论

用本文方法经过实验得到如下结论：

● A，B，C取代三角函数表达式，数学模型简单，从而提高处理速度

● 考虑在两个坐标系中的误差,6DOF 更接近真实值

● 计算后，结果存储在坐标转换矩阵中，而不是传统的矩阵R，偏移参数，旋转因子，等等。从而提高了相关处理的效率。

参考文献：

[1] Lim, E.H., Suter, D. 2009. 3D terrestrial LIDAR classifications with super-voxels and multi-scale conditional random fields. Comput. Aided Design 41, 701–710.

[2] Andreasson, H., Magnusson, M., Lilienthal, A. 2007. Has something changed here? Autonomous difference detection for security patrol robots. In: Proc. IEEE/RSJ Internat. Conf. on Intelligent Robots and Systems, San Diego (USA), pp: 3429– 3435.

[3] Cole, D.M., Harrison, A.R., Newman, P.M. 2005. Using naturally salient regions for SLAM with 3D laser data. In: Proc. IEEE Workshop on SLAM (In Conjunction with ICRA), Barcelona (Spain).

[4] Katz, R., Melkumyan, N., Guivant, J., Bailey, T., Nieto, J., Nebot, E. 2006. Integrated sensing framework for 3D mapping in outdoor navigation. In: IEEE/RSJ Internat.Conf. on Intelligent Robots and Systems, Beijing, China, pp: 2264–2269.

[5] 姚吉利，韩保民，杨元喜.罗德里格矩阵在三维坐标转换严密解算中的应用[J].武汉大学学报信息科学版，2006,12(24):1094-1096

[6] A. Pesonen , M. Maltamo, P. Packale. 2008. Airborne laser scanning-based prediction of coarse woody debris volumes in a conservation area.Forest Ecology and Management. ISPRS Journal of Photogrammetry & RemoteSensing 61 (2006) 33–46.

[7] Stamos, I., Leordeanu, M. 2003. Automated feature-based range registration of urban cenes of large scale. In: IEEE Computer Society Conference on Computer Vision and Pattern ecognition, Madison (USA), pp:555–561.

[8] Jensen, B., Weingarten, J., Kolski, S., Siegwart, R. 2005. Laser range imaging using mobile robots: From pose estimation to 3D-models. In: Proc. 1st Range Imaging Research Day, Zurich, Switzerland, pp: 129–144.

[9] Besl, P.J., McKay, N.D. 1992. A method for registration of 3-D shapes. IEEE Trans. Pattern Anal. Machine Intell. 14 (2), 239–256.

[10] Robertson, C., Fisher, R.B. 2002. Parallel evolutionary registration of range data.Computer Vision and Image Understanding 87, 39–50.

[11] Rusinkiewicz, S., Levoy, M. 2001. Efficient variants of the ICP algorithm. In: Proc. 3th Internat. Conf. on 3D Digital Imaging and Modeling, Quebec (Canada), pp:145–152.

[12] Paul J. Bed, Member，IEEE, and Neil D. McKay 1992. A Method for Registration of 3-D Shapes.Ieee transactions on parern analysis and machine intelligence, vol. 14, no. 2, February 1992.pp: 0162-8828.

[13] Zeng Wenxian,Tao Benzao. 2003 Non-linear model of 3D coordinate ransformation. Geomatics and Information Science of Wuhan University, 28(5):566-568. (in Chinese)

[14] 姚吉利.3维坐标转换参数直接计算的严密公式[J].测绘通报，2006.5:7-10

本文来源：https://www.2haoxitong.net/k/doc/1b735030a32d7375a417805c.html