[发明专利]一种惯性定位与点云配准耦合互补的实时三维重建方法

摘要

本发明公开了一种惯性定位与点云配准耦合互补的实时三维重建方法，该方法将获取的目标三维点云数据进行粗配准和精配准的过程，与惯性传感器获取的辅助惯性定位信息进行耦合互补，给出精确配准的目标三维点云数据，完成三维重建过程；本发明的方法能够有效地抑制惯性定位的发散，并提升三维点云数据配准的效率，减少配准过程中的运算，使三维重建过程达到较高的实时性和准确性。

主权项

1.一种惯性定位与点云配准耦合互补的实时三维重建方法，具体步骤如下：(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)。