对偶四元数法在稳健点云配准中的应用

李明峰，陆海芳，赵湘玉

(南京工业大学测绘科学与技术学院，江苏 南京 211800)

点云配准是三维激光扫描数据处理的关键步骤之一，其配准精度将直接影响后续数据处理及三维建模质量。点云配准的实质是将某一坐标系下的数据转换到另一坐标系，该过程可视为刚体的旋转和平移。刚体运动估计方法分为迭代法和解析法，迭代法通过构造欧拉角、正交矩阵或单位四元数形式的旋转矩阵，建立牛顿迭代模型，求解三维坐标转换参数。该方法计算精度高，但解算过程烦琐，计算效率较低，且对初值选取要求较高，选取不当易造成迭代结果不收敛；解析法先通过奇异值分解、正交矩阵或单位四元数法求解旋转矩阵，再获取平移向量。该方法计算效率高，但由于分步求解旋转矩阵和平移向量，易产生耦合误差，因此计算精度较低[1-4]。

研究表明，基于对偶四元数的点云配准方法能够统一描述刚体旋转和平移运动，直接求解旋转矩阵和平移向量，避免了旋转和平移参数分步求解产生的耦合误差[5]，计算精度高且无需迭代，计算效率高，是一种简便高效的点云配准方法。但该方法并未顾及参与运算的点云特征点含有粗差的情况。

为克服粗差影响，本文结合稳健估计中选权迭代思想，提出一种基于对偶四元数的稳健点云配准方法。该方法首先遵循加权最小二乘原则，构建基于对偶四元数的稳健点云配准模型；然后运用拉格朗日乘数法推导转换参数计算公式，并根据矩阵最大特征值对应的特征向量求解旋转矩阵和平移向量；根据坐标转换残差重新确定各公共特征点的权值，进行下一轮参数计算，反复迭代，直至相邻两次参数估计值之差在限值范围内为止；最终求得转换参数在抗粗差前提下的最优估计。

1 基于对偶四元数的稳健点云配准模型

对偶四元数是元素为四元数的对偶数，其表达式为

(1)

(2)

式中，R为旋转矩阵；T为平移向量;

(3)

设待配准点云集合中的特征点为Di(Xi,Yi,Zi)T，目标点云集合中对应特征点为DTi(XTi,YTi,ZTi)T，i=0,1,2,…,n表示两个点云集合中第i对同名特征点，n表示同名特征点对数。点云坐标转换通用模型为

DTi=RDi+T

(4)

对于每一对同名特征点，进行坐标转换后存在误差vi=(vxi,vyi,vzi)，vi=RDi+T-DTi，即

(5)

利用式(2)将式(5)表达成对偶四元数形式

(6)

基于加权最小二乘原则，进行点云配准的结果就是使点云集合中所有特征点的加权误差平方和最小，即令下式误差函数值最小

(7)

2 基于对偶四元数的稳健点云配准方法

2.1 稳健点云配准参数解算

将式(7)展开，整理可得

(8)

结合式(3)，采用拉格朗日求极值法，则新的目标函数为

(9)

(10)

(11)

(12)

将式(12)代入式(10)，计算可得

(13)

(14)

(15)

将式(14)和式(15)代入式(8)，化简得

F(R,T)=C-λ1

(16)

2.2 稳健点云配准权函数

权因子构造函数简称权函数，是基于对偶四元数的稳健点云配准方法的关键，常用权函数有:

(1) Huber函数

(17)

(2) Danish函数

(18)

(3) IGGⅢ函数

(19)

2.3 稳健点云配准解算步骤

基于对偶四元数的稳健点云配准方法解算步骤如下：

(1) 读取原始数据：公共特征点在待配准点云集合中坐标Di(Xi,Yi,Zi)T及其对应目标点云集合中坐标DTi(XTi,YTi,ZTi)T。

(3) 根据式(2)组成旋转矩阵R和平移向量T。

(4) 根据式(6)计算坐标转换残差，并根据所选权函数重新计算各公共特征点的权因子，构造新的等价权元素。

(5) 将新的等价权元素代入式(13)，重复步骤(2)～(4)，当相邻两次坐标转换参数差值均在限差范围内时，停止迭代。

3 试验验证

为了验证基于对偶四元数的稳健点云配准方法的实用性，采用Focus3DX330型三维激光扫描仪对布设球形标靶的某场景进行扫描，利用FARO Scene软件提取相邻两测站对应靶球的中心坐标见表1。

表1 相邻两测站靶球中心坐标 m

分别采用对偶四元数法(DQ)和基于对偶四元数的稳健点云配准方法(RDQ)解算转换参数，RDQ法中权函数依次运用Huber函数(Huber-DQ)、Danish函数(Danish-DQ)和IGGⅢ函数(IGGⅢ-DQ)，各方法的解算精度σ0和迭代次数p见表2。

由表2可知：

(1) RDQ的3种方法中，Huber-DQ法和Danish-DQ法k0=1.0、IGGⅢ-DQ法k0=1.0，且k1=2.5时，各方法对应解算精度最高，最高解算精度均高于DQ法，其中IGGⅢ-DQ法最高解算精度高于Danish-DQ法，Danish-DQ法最高解算精度高于Huber-DQ法；当3种方法各自的调和系数在合理范围内取其他值时，解算精度均与DQ法相同。

(2) 从各个方法最高解算精度对应的迭代次数可看出，Huber-DQ法、Danish-DQ法和IGGⅢ-DQ法均牺牲了计算效率，提高了计算精度。

为了验证RDQ法的抗差性，给待配准坐标系下的标靶中心坐标依次加入粗差(0.008,0.010,0.012)m(点位误差为17.55 mm)，分别运用DQ法、Huber-DQ法、Danish-DQ法和IGGⅢ-DQ法计算粗差在不同位置下的转换精度(σ0)和迭代次数(p)，结果见表3。粗差在不同位置下各个标靶的点位误差如图1所示。

由表3可知：

(1) 在特征点数据含粗差情况下，DQ法解算精度大大降低；无论粗差点位于何位置，RDQ法的解算精度均高于DQ法，其中IGGⅢ-DQ法精度略高于Danish-DQ法，Danish-DQ法精度显著高于Huber-DQ法。说明通过选权迭代，粗差影响减较小，解算精度提高；相比Huber-DQ法，IGGⅢ-DQ法和Danish-DQ法抗差能力更强。

表2 各方法的转换精度与迭代次数

表3 粗差在不同位置下各方法的解算精度及迭代次数

(2) 就迭代收敛效率而言，IGGⅢ-DQ法和Danish-DQ法迭代次数相当，均少于Huber-DQ法。说明IGGⅢ-DQ法和Danish-DQ法解算效率相当，均优于Huber-DQ法。

由图1可知：无论粗差点位于何位置，相较于DQ法，RDQ的3种方法均更能有效定位粗差位置，其中IGGⅢ-DQ法和Danish-DQ法不仅能够准确定位粗差点，而且计算得到的点位误差与加入的粗差相近，不含粗差的点，其点位误差也相对较小。

4 结 语

本文针对点云特征点含有粗差的问题，遵循加权最小二乘原则，结合稳健估计中选权迭代思想，提出了基于对偶四元数的稳健点云配准方法，试验结果表明：

(1) 在基于对偶四元数的稳健点云配准方法中，Huber-DQ法识别和剔除粗差的能力较弱且解算效率较低，IGGⅢ-DQ法和Danish-DQ法不仅具有较强的粗差识别和剔除能力，解算效率也相对较高。

(2) 基于对偶四元数的稳健点云配准方法克服了对偶四元数法抗差性弱的缺点，而且在不含粗差的情况下提高了点云配准精度，是一种更有效的点云配准方法，具有良好的应用价值。