一种惯性定位与点云配准耦合互补的实时三维重建方法与流程

本发明涉及三维点云数据重建，具体涉及一种惯性定位与点云配准耦合互补的实时三维重建方法。

背景技术：

三维扫描及三维重建技术的出现为计算机图形学、机器视觉、工业设计、医疗诊断、虚拟实境、增强现实、逆向工程等一系列研究和实际应用提供了可能。该相关领域内的内容核心是如何让三维扫描技术获得的数据得到高效及精确的配准。

所谓三维点云数据配准，指的是对于从不同位置角度对同一物体得到的不同的点云，找出它们之间的实际空间关系。一般地，主流的配准方法使用粗配准到精配准的流程，从不同位置的传感器得到的同一物体的点云数据中提取信息，通过RANSAC等粗配准算法，找到对应程度较高的两片点云，通过ICP算法迭代得到这两片点云的刚体变换关系，从而实现配准。在这个算法过程中，由点云自身提供的信息来引导点云配准迭代过程的收敛，往往导致庞大的计算量，从而影响实际配准过程的速度，难以在保持精度的前提下达到一定的实时性。

惯性器件直接获得加速度信息，在定位的应用上，通过积分来确定速度和位移会造成误差不断积累，最终导致结果发散，单方面利用惯性器件难以有效为三维重建提供帮助。如果采用零速校正等方案补偿累积误差的方案，又会要求传感器的运动过程受到过多限制，极大削弱实时性和便利性。

专利号为CN105261060A的专利公布了一种基于点云压缩和惯性导航的移动场景实时三维重构方法，该方法分为三个阶段：第1阶段，设计一种基于PFH的点云压缩算法；第2阶段，设计了一种基于惯性导航的点云配准算法；第3阶段，完成点云融合；该方法只是将惯性器件获得的加速度值进行一次积分和二次积分得到的结果分别作为惯性导航的速度和位移数据，并以此来确定RGB-D传感器的实际位置姿态。采用这样的方法，会导致随着三维重构过程的进行，对RGB-D传感器的位置和姿态的估计结果出现严重的发散，使得估计结果与实际结果出现较大偏差，进而使惯性定位数据失去对点云配准计算的约束作用。

技术实现要素：

针对惯性定位发散与三维点云数据配准的效率问题，本发明提出了一种惯性定位与点云配准耦合互补的实时三维重建方法，本发明对惯性定位与点云配准方法的优点进行耦合，利用互补反馈机制，既保证了三维重建精度，又提升了整个三维重建过程的实时性。

一种惯性定位与点云配准耦合互补的实时三维重建方法，具体步骤如下：

(1)采集初始惯性定位数据及初始点云数据；

(2)对初始点云数据进行预处理；

(3)将初始惯性定位数据记为原始加速度和原始四元数；

(4)采集相机的当前加速度、当前四元数及当前时刻的点云数据；

(5)若在初始循环中，则将当前加速度、当前四元数、原始加速度以及原始四元数一起处理，得到零时先验平移变换和零时先验转动变换矩阵，然后将零时先验平移变换和转动变换矩阵作为系统的先验变换矩阵；若不在初始循环中，则将上一时刻的步骤(13)中的后续先验变换矩阵作为系统的先验变换矩阵；

(6)根据先验变换矩阵将待配准点云逆变换为新的待配准点云；根据惯性数据约束，在新的待配准点云与参考点云中，将原始三维物体表面不重合的部分去掉，剩下的为有效待配准点云和有效参考点云；

(7)判断有效待配准点云和有效参考点云之间的重叠区域是否大于阈值，若否，进入步骤(8)，若是，进入步骤(9′)；

(8)使用有效待配准点云和有效参考点云，进行点云数据粗配准，得到刚体变换矩阵；

(9)将刚体变换矩阵转换的位移和四元数作为观测量进行卡尔曼滤波，得到位移和四元数后验估计值，并转换为后验变换矩阵；将后验变换矩阵作为约束矩阵，进入步骤(10)；

(9′)直接将先验变换矩阵作为约束矩阵，然后进入步骤(10)；

(10)将约束矩阵作为ICP算法的初始值开始迭代计算，进行点云数据精配准；得到迭代收敛变换矩阵；

(11)待配准点云数据根据迭代收敛变换矩阵进行逆变换，得到优化的点云数据，并进行三维重建；

(12)根据实时三维重建反馈结果，判断三维重建是否完成，即用户是否触发终止命令，如果是，终止计算，如果否，继续步骤(13)；

(13)将迭代收敛变换矩阵转换的位移和四元数作为观测量进行卡尔曼滤波，得到位移优化后验估计值和四元数优化后验估计值，由位移和四元数的优化后验估计值进行时间更新，预测下一时间步的位移后续先验估计值和四元数后续先验估计值；

(14)将位移和四元数的后续先验估计值转换为后续先验变换矩阵；

(15)循环步骤(4)～步骤(12)。

在步骤(2)中，将初始点云数据通过基于体素栅格的采样方式变换为TSDF(truncated signed distance field)数据。

在步骤(3)中，记原始加速度为a_ini＝(a_x,ini a_y,ini a_z,ini)^T，记原始四元数为q_ini＝(q_0,ini q_1,ini q_2,ini q_3,ini)^T。

在步骤(4)中，记当前加速度为a_cur＝(a_x,cur a_y,cur a_z,cur)^T，记当前四元数为q_cur＝(q_0,cur q_1,cur q_2,cur q_3,cur)^T。

在步骤(5)中，若在初始循环中，则将步骤(3)和步骤(4)中的数据进行处理，得到零时先验平移变换和先验转动变换，具体方法为：

步骤(3)和步骤(4)中的原始四元数和当前四元数所对应的转动变换矩阵的逆矩阵分别为：

世界坐标系下的原始加速度和当前加速度分别为：

a_ini^W＝R_ini^-1a_ini

a_cur^W＝R_cur^-1a_cur

零时先验平移变换为其中△t为第一步时间间隔；

零时先验转动变换为R_pr(q_ini,q_cur)＝R_curR_ini^-1；

先验变换矩阵为[T_pr(a_ini,a_cur)|R_pr(q_ini,q_cur)]。

在步骤(5)中，先验变换矩阵为传感器位置变化后待配准点云提供了其所在坐标系的位置姿态约束条件。

在步骤(7)中，判断有效待配准点云和有效参考点云间重叠区域大小的具体方法是：在通过先验变换矩阵的待配准点云中随机取点，根据该点与参考点云下采样组成的平面是相交、在前或是在后取不同的采样值，根据采样值求平均取极限来估计重叠区域占比；判断阈值取20％～80％，判断阈值可以根据具体设备参数和配准结果精度可做调整。

在步骤(8)中，使用经过先验变换矩阵确定的有效待配准点云进行点云数据粗配准，相当于在粗配准中引入了来自于先验变换矩阵的位置姿态约束条件。

在步骤(8)中，当采取惯性数据约束下的基于RANSAC的方法进行粗配准时，具体步骤为：

(8-1)在有效待配准点云和有效参考点云中的点按照法向量特征分层，如果某个点的法向量在整个点云的法向量空间中出现的频率越低，就将该点分配到优先级越高的层；

(8-2)在有效待配准点云中取三点为一组，寻找在准重合参考点云中的对应点组，选取点按照(8-1)建立的分层机制，从优先级高的层逐渐往优先级低的层进行；

(8-3)找到对应点组之后，以对应点之间距离平方和最小为原则，采用SVD法计算出当前点组所对应的刚体变换及点间距离平方和；

(8-4)当点间距离平方和小于阈值时，基于RANSAC的配准结束；阈值初始设定为10l，l为RGB-D传感器物理极限精度，后续根据实验反馈可调整阈值的取值。

在步骤(8)中，当采取惯性数据约束下的基于PCA的方法进行粗配准时，具体步骤为：

(8-1′)将有效待配准点云P′和有效参考点云P的点坐标分别记为：

p′_i＝(p′_ix p′_iy p′_iz)^T

p_i＝(p_ix p_iy p_iz)^T

其中i表示第i个点；

求有效待配准点云P′和有效参考点云P的重心坐标分别为：

其中N_P′和N_P分别为有效待配准点云和有效参考点云的点个数；

(8-2′)以v表示采集点云数据的传感器的视角矢量，以n′_i和n_i分别表示有效待配准点云和有效参考点云中第i个点的法向量，求有效待配准点云和有效参考点云中的每个点的权重分别为：

求和得：

计入权重的重心为：

(8-3′)计算协方差矩阵如下：

将协方差矩阵进行SVD奇异值分解：

K′＝U′D′U′^T

K＝UDU^T

其中D′和D是对角矩阵；U′和U为3阶归一化特征向量矩阵。

则转动刚体变换R′和平移刚体变换T′分别为：

R′＝U′U^-1

在步骤(9)中，将当前加速度作为加速度观测量，将由刚体变换矩阵转换的位移作为位移观测量，将由刚体变换矩阵转换的转动四元数作为四元数观测量，进行卡尔曼滤波，得到位移后验估计值与四元数后验估计值，并将位移后验估计值与四元数后验估计值转换为后验变换矩阵。

在步骤(9)中，为了抑制惯性导航的误差累积，将由刚体变换矩阵转换的位移与转动观测量进行卡尔曼滤波，即将先验变换矩阵通过点云粗配准的约束转化为较为精确的后验变换矩阵。

在步骤(9)中，卡尔曼滤波公式为：

x_k+1＝A_kx_k+Bu_k+w_k

z_k＝H_kx_k+v_k

其中，x是状态矢量，A是状态转移矩阵，B是控制矩阵，u是系统输入矢量，w是协方差阵为Q的过程噪声矢量；z是观测矢量，H是系统观测矩阵，v是协方差阵为W的量测噪声矢量；k表示该矢量或矩阵，是滤波过程的第k步对应的矢量或矩阵。

在步骤(10)中，将约束矩阵作为约束条件，将使ICP迭代计算能更快收敛，结果更为精确。

在步骤(11)中，待配准点云数据根据迭代收敛变换矩阵转换到世界坐标系下，并进行三维重建过程。

在步骤(13)中，将迭代收敛变换矩阵对应的位移和四元数作为观测量，使用卡尔曼滤波对相机此时相对于初始时刻的位移与四元数的优化后验估计值做出估计，然后根据此优化后验估计值预测下一时刻的位移与四元数的后续先验估计值。

在步骤(13)中，反馈的观测量代替单纯的惯性数据的积分来作为估计的依据，通过卡尔曼滤波器估计当前时刻位移和四元数的优化后验估计值，可以有效抑制误差累积导致的发散。

在步骤(13)中，依据卡尔曼滤波原理得出位移后续先验估计值与四元数后续先验估计值的具体方法为：

其中为先验估计值建立的状态矢量，为后验估计值建立的状态矢量。A是状态转移矩阵，B是控制矩阵，u是系统输入矢量；k表示该矢量或矩阵是当前时刻对应的矢量或矩阵，k+1表示该矢量或矩阵是下一时刻对应的矢量或矩阵。

在步骤(14)中，将步骤(13)得到的位移和四元数的后续先验估计值转换为后续先验变换矩阵，此时的后续先验变换矩阵是基于上一时位移和四元数的优化后验估计值得到的；上一时刻的位移和四元数的优化后验估计值是将迭代收敛变换矩阵对应的位移和四元数值作为观测量进行卡尔曼滤波得到的。

本发明将传感器获取的目标三维点云数据进行粗配准和精配准的过程，与惯性传感器获取的辅助定位信息进行耦合互补，既能够很好地抑制误差，又能提高三维重建的精度与速度。

在整个方法流程中，辅助惯性定位数据为配准提供优化的约束条件，配准结果为惯性定位提供收敛的观测值，二者合理地耦合互补，使整个三维重建过程实时跟踪传感器位置姿态，并给出精确配准过的目标三维点云数据。

本发明的方法能够有效地抑制惯性定位的发散，并提升三维点云数据配准的效率，减少配准过程中的运算，使三维重建过程达到较高的实时性和准确性。

附图说明

图1为本发明实时三维重建方法的流程图；

图2为TSDF算法融合三维点云原始数据的示意图；

图3为本发明实时三维重建方法的过程示意图。

具体实施方式

为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。

如图1所示，本发明惯性定位与点云配准耦合互补的实时三维重建方法的具体步骤如下：

(1)陀螺仪和加计组成的惯性单元获取初始时刻相机加速度a_ini＝(a_x,ini a_y,inia_z,ini)^T、四元数q_ini＝(q_0,ini q_1,ini q_2,ini q_3,ini)^T；RGB-D传感器获得初始时刻点云数据；

对点云数据进行处理，将点云数据通过基于体素栅格的采样方式变换为truncated signed distance field(TSDF)数据，该方式原理示意图如图2所示：

记原有的栅格距离值为D_i(x)，权重为W_i(x)，新加入的栅格距离值为d_i+1(x)，权重为w_i+1(x)，则新的TSDF数据按下式融合：

W_i+1(x)＝W_i(x)+w_i+1(x)；

其中，D_i+1(x)为当前点云对应的栅格距离值，W_i+1(x)为当前点云的权重。

(2)采集当前时刻相机的加速度a_cur、四元数q_cur及传感器的当前点云数据。

(3)确定先验变换矩阵先验变换矩阵根据当前循环是否处于初始循环代表不同意义：若当前循环为初始时刻的循环，则将当前时刻的惯性定位数据与原始加速度和原始四元数一起处理，得到零时先验平移变换和零时先验转动变换矩阵，然后将零时先验平移变换和转动变换矩阵作为系统的先验变换矩阵若当前循环不是初始时刻的循环，先验变换矩阵取上一时刻的步骤(12)中的后续先验变换矩阵

求零时先验变换矩阵的具体操作为：将当前时刻的加速度a_cur和四元

数数据q_cur与原始加速度a_ini和原始四元数数据q_ini一起处理，转换成零时先验变换矩阵其中R_pr(q_ini,q_cur)为零时先验转动变换矩阵，是原始四元数q_ini和当前转四元数q_cur的矩阵，由原始转动矩阵R_ini和当前转动矩阵R_cur求得，T_pr(a_ini,a_cur)为零时先验平移变换矩阵，是世界坐标系下的原始加速度a_ini^W和世界坐标系下的当前加速度a_cur^W的矩阵。

其中，原始转动矩阵和当前转动矩阵由下式得到：

两个矩阵的逆矩阵直接通过矩阵转置求得：

R_ini^-1＝R_ini^T

R_cur^-1＝R_cur^T

先验转动变换矩阵按照下式得到：

R_pr(q_ini,q_cur)＝R_curR_ini^-1

世界坐标系下的原始加速度和当前加速度由下式得到：

a_ini^W＝R_ini^-1a_ini

a_cur^W＝R_cur^-1a_cur

平移变换矩阵T_pr由是世界坐标系下的原始加速度a_ini^W和世界坐标系下的当前加速度a_cur^W按照下式得到：

其中△t为第一步时间间隔；

先验变换矩阵为[T_pr(a_ini,a_cur)|R_pr(q_ini,q_cur)]。

(4)根据先验变换矩阵将待配准点云逆变换为新的待配准点云；根据惯性数据约束，在新的待配准点云与参考点云中，将原始三维物体表面不重合的部分去掉，剩下有效待配准点云和有效参考点云N_P′即为点云P′中点的个数，N_P即为点云P中点的个数。

(5)判断有效待配准点云和有效参考点云之间的重叠区域是否大于阈值，若否，进入步骤(6)，若是，进入步骤(8*)。

(6)根据有效参考点云P、有效待配准点云P′以及先验变换矩阵进行点云数据粗配准，得到刚体变换矩阵[R′|T′]；

刚体变换矩阵[R′|T′]中的R′通过将矩阵中的元素进行运算可得到四元数q′的值。

刚体变换矩阵[R′|T′]中的T′是位移S′的矩阵。

体素栅格粗配准采用基于RANSAC法的方法或优化的PCA法，惯性单元提供的先验变换矩阵可以对所选的采样点云数据提供范围限制约束，从而更加快速准确地得出刚体变换矩阵[R′|T′]。

(7)将刚体变换矩阵[R′|T′]转换为当前时刻相对初始时刻的位移S′和四元数q′；

记R′为：

记q′为：

q′＝(q′₀ q′₁ q′₂ q′₃)^T

则求取q′方法为：

先求绝对值：

确定的符号的方法为：

sign(q′₁)＝sign(q′₀)[sign(R′₃₂-R′₂₃)]

sign(q′₂)＝sign(q′₀)[sign(R′₁₃-R′₃₁)]

sign(q′₃)＝sign(q′₀)[sign(R′₂₁-R′₁₂)]

q′₀的符号可任取；

记T′为：

T′＝(T′_x T′_y T′_z)^T

位移S′有：

S′＝(S′_x S′_y S′_z)^T＝T′＝(T′_x T′_y T′_z)^T

(8)根据加速度a_cur、位移S′和四元数q′进行卡尔曼滤波计算，得到位移后验估计值S_eva和四元数后验估计值q_eva；并将位移后验估计值S_eva和四元数后验估计值q_eva转换为后验变换矩阵[R_eva(q_eva)|T_eva(S_eva)]；

其中，后验变换矩阵[R_eva(q_eva)|T_eva(S_eva)]中的R_eva(q_eva)为四元数后验估计值q_eva的矩阵，后验变换矩阵[R_eva(q_eva)|T_eva(S_eva)]中的T_eva(S_eva)为位移后验估计值S_eva的矩阵。

卡尔曼滤波公式：

x_k+1＝A_kx_k+Bu_k+w_k

z_k＝H_kx_k+v_k

其中，x是状态矢量，A是状态转移矩阵，B是控制矩阵，u是系统输入矢量，w是协方差阵为Q的过程噪声矢量；z是观测矢量，H是系统观测矩阵，v是协方差阵为W的量测噪声矢量；k表示该矢量或矩阵，是滤波过程的第k步对应的矢量或矩阵。

得到后验变换矩阵[R_eva(q_eva)|T_eva(S_eva)]之后，将其作为约束矩阵，进入步骤(9)的计算。

(8*)将先验变换矩阵直接作为约束矩阵，进入步骤(9)的计算。

(9)以约束矩阵作为约束条件，通过ICP算法进行点云数据精配准，得到平移变换矩阵T″和转动四元数q″，并将转动四元数q″转换为转动变换矩阵R″，平移变换矩阵T″与转动变换矩阵R″构成迭代收敛变换矩阵[R″|T″]；

其中，转动变换矩阵R″是元素四元数q″的矩阵，平移变换矩阵可以直接转化为位移S″；

ICP算法在需配准的两片点云中选取对应点，以两片点云上所有对应点之间平方和最小为标准，通过迭代得到两片点云间精确的转动变换矩阵R″；

对于点云数据P和点云数据P′，计算所有对应点平方和的平均的公式如下(以角标一样表示对应的点)：

其中，R和T的初始输入为上述约束条件[R_eva|T_eva]，N_p′为点云P′中的点的个数，若第k次迭代刚体变换与第k-1次迭代的点对距离平方和满足关系||d_k-d_k-1||<ε，ε为设定的阈值(ε的范围初始设置为l，l为RGB-D传感器物理极限精度，后续根据实验反馈可调整ε取值)，则此时的刚体变换为精确的迭代收敛变换矩阵[R″|T″]。

(10)根据迭代收敛变换矩阵[R″|T″]计算点云数据P′在世界坐标系下的对应点云数据P″，并利用点云数据P″进行三维重建。

(11)根据实时三维重建反馈结果，判断三维重建是否完成，即用户是否出发终止命令，如果是，终止计算，如果否，继续步骤(12)；

(12)将迭代收敛变换矩阵对应的位移S″和四元数q″作为观测量，通过卡尔曼滤波，对RGB-D传感器相对于初始时刻的位移优化后验估计值和四元数优化后验估计值进行估计，然后以优化后验估计值和对下一时刻的位移和四元数的先验估计值做出预测，得到相对于初始时刻的位移后续先验估计值与四元数后续先验估计值

在步骤(12)中，通过卡尔曼滤波估计优化后验估计值和时，将迭代收敛变换矩阵对应的位移S″和转动四元数q″作为观测矢量代入如步骤(8)所述的卡尔曼滤波公式。

在步骤(12)中，依据卡尔曼滤波原理得出位移后续先验估计值与四元数后续先验估计值的具体方法为：

其中为先验估计值建立的状态矢量，为后验估计值建立的状态矢量。A是状态转移矩阵，B是控制矩阵，u是系统输入矢量；k表示该矢量或矩阵是当前时刻对应的矢量或矩阵，k+1表示该矢量或矩阵是下一时刻对应的矢量或矩阵。

该步骤的重要性在于，反馈的观测量代替单纯的惯性数据的积分来估计传感器的速度和位移的优化后验估计值，抑制误差累积导致的发散，使得对下一时刻的位移和四元数的先验估计结果依然可靠，适用于作为配准的参照值。

(13)将位移后续先验估计值与四元数后续先验估计值转换为后续先验变换矩阵

其中转动变换矩阵是四元数后续先验估计值的矩阵，平移变换矩阵是位移后续先验估计值的矩阵。

(14)循环步骤(2)～步骤(11)。

在进行一个相对完整的三维重建过程中，本算法的效率相比skanect等市场上主流的三维扫描软件提高了一倍以上。

以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的最优选实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。