CN108692661A - 基于惯性测量单元的便携式三维测量系统及其测量方法 - Google Patents

Abstract

本发明适用于三维传感及测量领域，提供了一种基于惯性测量单元的便携式三维测量系统及其测量方法，其中该便携式三维测量系统包括三维测量单元和惯性测量单元：惯性测量单元，固定在所述三维测量单元上，用于获取所述三维测量单元的自身相对位置；三维测量单元，用于通过投影辅助对被测物体进行测量，获取所述被测物体表面局部的点云数据。本发明实施例提供的便携式三维测量系统包括惯性测量单元，通过惯性测量单元实时获取自身的相对位置，结合预先的位姿标定，就能够获得三维测量单元的位姿信息，将其应用到ICP迭代的初始值上，进而实现点云高精度配准。

Description

基于惯性测量单元的便携式三维测量系统及其测量方法

技术领域

本发明属于三维传感和测量领域，尤其涉及一种基于惯性测量单元的便携式三维测量系统及其测量方法。

背景技术

三维重建技术在工业生产及检测、游戏建模、服饰行业、文物保护以及医学工程等领域具有广泛的应用前景。基于机械、光学、声学、电磁学原理的各种测量方法均取得了巨大进展，尤其是以激光扫描法、干涉法和结构光方法为代表的光学非接触测量方法，在许多领域中实现了商业应用。

基于双目系统的结构光三维重建，利用系统的投影装置向待测物体投射单幅或者多幅结构光图像，系统的两个相机同步获取经结构光编码的待测物体图像，对图像进行信息处理，建立左右两相机图像的对应关系，进而实现三维点云重建。典型的结构光三维重建方法有：散斑相关方法、相位测量轮廓术方法等。

当被测物体尺度较大、存在遮挡或内外表面时，为获取物体的完整三维表达，单视点测量系统一般需借助辅助运动机构如机械臂、转台等进行测量，或者构建三维传感器测量网络，使测量空间全面覆盖被测物体空间。上述两种方法均导致测量系统成本增加，且不灵活。一种低成本的解决方案是，对于能够实时测量的单视点三维测量系统，因不同测量视角获取的点云数据间的错动较小，一般可以不提供配准初值，直接对不同视角的测量点云使用ICP(Iterative Closest Point)迭代算法进行点云配准，但通常稳定性较差，容易出现误匹配现象。

综上所述，目前的三维测量系统，大多数无法实现实时测量，且系统成本高，测量过程不灵活。

发明内容

本发明所要解决的技术问题在于提供一种基于惯性测量单元的便携式三维测量系统及其测量方法，旨在解决现有三维测量系统无法实现实时测量，且系统成本高，测量过程不灵活的问题。

本发明是这样实现的，一种基于惯性测量单元的便携式三维测量系统，包括三维测量单元和惯性测量单元：

所述惯性测量单元，固定在所述三维测量单元上，用于获取所述三维测量单元的自身相对位置；

所述三维测量单元，用于通过投影辅助对被测物体进行测量，获取所述被测物体表面局部的点云数据。

进一步地，所述三维测量单元包括两个相机和一个投影模块；

所述投影模块，用于向所述被测物体表面投射结构光图像；

所述相机，用于采集经过所述被测物体表面调制的结构光图像信息。

进一步地，所述惯性测量单元包括三轴陀螺仪和三轴加速度计。

本发明实施例还提供了一种如上述所述的便携式三维测量系统的测量方法，包括：

步骤A，对所述便携式三维测量系统进行标定，得到所述三维测量单元和所述惯性测量单元之间的位姿变换关系，并以所述惯性测量单元各姿态角度输出为0时，所述三维测量单元的坐标系作为全局坐标系；

步骤B，利用所述三维测量单元对被测物体进行测量，得到所述被测物体表面局部的点云数据，并利用所述惯性测量单元的输出信息解算其位姿信息；

步骤C，保持所述被测物体不动，变换测量视角，利用所述三维测量单元对所述被测物体进行测量，并利用所述惯性测量单元的输出信息解算此时惯性测量单元的位姿信息，所述三维测量单元在当前测量视角和前一测量视角下获取的点云数据存在共同区域；

步骤D，计算所述惯性测量单元在当时测量视角相对于前一测量视角的位姿变换关系，并计算两次测量时所述三维测量单元的位姿变换关系；

步骤E，将前一测量视角的三维测量单元坐标系到全局坐标系的精确位姿变换关系和两次测量的三维测量单元的位姿变换关系应用于当前测量视角的点云迭代最近点ICP配准的迭代初始值，并进行迭代最近点ICP迭代；

步骤F，在迭代最近点ICP迭代结束后，得到所述三维测量单元在当时测量视角与前一测量视角之间的精确位姿变换关系，并用所述三维测量单元的精确位姿变换关系对当前所述惯性测量单元的位姿进行调整；

步骤G，重复步骤C到步骤F，直至完成对所述被测物体的测量；

步骤H，对测量过程中的获取的点云数据进行优化处理，得到点云模型。

进一步地，所述步骤A中，对所述便携式三维测量系统进行标定包括对两个相机的标定和对所述惯性测量单元位姿的标定，其中：

对两个相机的标定包括：获取两个相机的内部参数、镜头畸变参数及两个相机坐标系间的刚体变换；

对所述惯性测量单元位姿的标定包括：计算所述惯性测量单元的坐标系相对于三维测量单元的坐标系的刚体变换T_ic,其中，R_ic表示旋转变换，t_ic表示平移向量。

进一步地，所述利用所述三维测量单元对被测物体进行测量，得到所述被测物体表面局部的点云数据包括：

采用散斑相关方法，利用所述三维测量单元向所述被测物体的表面投射结构光图像，两个相机采集经过所述被测物体的表面调制的结构光图像信息，利用所述结构光图像信息中的散斑信息进行同名点查找及亚像素优化，得到所述被测物体表面局部的点云数据；

或：

采用相位测量轮廓术方法，利用所述三维测量单元向所述被测物体的表面投射结构光图像，两个相机采集经过所述被测物体的表面调制的结构光图像信息，利用所述结构光图像信息中的相位信息进行同名点查找及亚像素优化，得到所述被测物体表面局部的点云数据。

进一步地，所述利用所述惯性测量单元的输出信息解算其位姿信息包括：

通过所述惯性测量单元的输出信息，解算惯性测量单元坐标系相对于其自身基准坐标系的刚体变换，根据所述惯性测量单元的刚体变换计算当前所述三维测量单元的位姿信息。

进一步地，此时惯性测量单元的位姿信息以T_m'₀表示，当前测量视角以m表示，前一测量视角以n表示，所述惯性测量单元在当时测量视角相对于前一测量视角的位姿变换关系以T_m'_n表示，两次测量时所述三维测量单元的位姿变换关系以T_mn表示；

根据T_m'₀＝T_n'₀T_m'_n，得到其中T_n'₀是前一测量视角n的位姿信息；

根据T_mnT_ic＝T_icT_m'_n，得到当前测量视角m相对于前一测量视角n的三维测量单元坐标系间的刚体变换T_mn，则

进一步地，迭代初始值以T_0m0表示，T_0m0＝T_n0T_mn，其中，T_n0为上一测量视角的ICP点云配准结果，步骤E具体包括：

使用截断符号距离函数结合光线投射的方式从预先建立的空间立方体渲染配准所需的目标点云；

最小化目标函数f(R,T)，即：

其中，p_i为目标点云中的点，q_i为源点云中与p_i相对应的点，为目标点云在p_i处的单位法向量，R为待优化的旋转变换，T为待优化的平移向量；

对待配准的点云数据进行下采样，构建多级点云数据；

将点数量较少的点云数据作为源点云进行ICP配准，并将得到的刚体变换作为下一级点云进行ICP配准的初始变换，以这种方式逐级进行迭代配准，直至到达最后一级点云，完成配准。

进一步地，所述用所述三维测量单元的精确位姿变换关系对当前所述惯性测量单元的位姿进行调整包括：

将所述三维测量单元的精确位姿变换关系应用到下一测量视角的ICP配准初始值上，以实现对惯性测量单元进行位姿调整。

本发明与现有技术相比，有益效果在于：本发明实施例提供的便携式三维测量系统包括惯性测量单元，通过惯性测量单元实时获取自身的相对位置，结合预先的位姿标定，就能够获得三维测量系统的位姿信息，将其应用到ICP迭代的初始值上，进而实现点云高精度配准。ICP配准完成后，利用配准结果对当前惯性测量单元的位姿进行校正，以减小惯性测量单元累积误差对测量的影响，保证下一测量视角ICP配准初始值的可靠性，提升点云配准的稳定性。

附图说明

图1是本发明实施例提供的一种基于惯性测量单元的便携式三维测量系统的结构示意图；

图2是本发明实施例提供的一种便携式三维测量系统的测量方法的流程图；

图3是本发明实施例提供的利用散斑相关方法重建的点云效果图；

图4是本发明实施例提供的不同测量视角下各坐标系间的变换关系示意图；

图5是本发明实施例提供的经过点云配准及后处理优化后的点云效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1示出了本发明实施例提供的一种基于惯性测量单元的便携式三维测量系统，包括三维测量单元101(图中未示出)和惯性测量单元102；惯性测量单元102，固定在三维测量单元101上，用于获取三维测量单元101的自身相对位置；三维测量单元101，用于通过投影辅助对被测物体进行测量，获取所述被测物体表面局部的点云数据。其中，三维测量单元101包括两个相机(即相机1和相机2)和一个投影模块3，投影模块3，用于向所述被测物体表面投射结构光图像；相机，用于采集经过所述被测物体表面调制的图像信息。惯性测量单元102包括三轴陀螺仪和三轴加速度计。

惯性测量单元(Inertial Measurement Unit，IMU)作为一项新型惯性导航领域发展起来的新技术，具有价格低、尺寸小、重量轻、功耗小、可靠性高等优点。微型惯性测量单元由微型加速度计、微型陀螺仪以及相应的集成电路和导航软件组成。将其安装在便携式三维测量设备上，使测量系统在改变测量视角时，能够自行确定其自身坐标系的相对空间位置，将其应用于ICP点云配准的迭代初始值，从而使便携式测量系统能够实现灵活、稳定、低成本、高精度的三维点云配准。

图2示出了图1所提供的便携式三维测量系统的测量方法，包括：

步骤S1，对基于惯性测量单元的便携式三维测量系统进行标定，获得惯性测量单元与三维测量单元之间的位姿变换关系，并以惯性测量单元各姿态角度输出为0时，三维测量单元坐标系为全局坐标系，图4中已标出；

具体地，基于惯性测量单元的便携式三维测量系统中包括两个相机、一个投影模块、一个惯性测量单元，如图1所示。由两个相机及一个投影模块组成一个主动式双目的三维测量单元，并且两个相机由投影模块投影时输出的触发信号同步外触发，用于实时重建三维点云数据。惯性测量单元固定在三维测量单元上，用于辅助获取三维测量单元的相对空间位置，进而实现点云配准。

本步骤中的标定包括两个相机标定及惯性测量单元位姿标定。其中，相机标定指获取两相机的内部参数、镜头畸变参数及两相机坐标系间的刚体变换；惯性测量单元位姿标定指计算惯性测量单元坐标系相对三维测量单元坐标系的刚体变换T_ic，标定过程类似手眼标定。其中：

其中R_ic为旋转变换，t_ic为平移向量。

在本步骤中，获取相应的刚体变换，即完成了相应部件的标定。

步骤S2，利用所述三维测量单元对被测物体进行测量，得到所述被测物体表面局部的点云数据，并利用所述惯性测量单元的输出信息解算其位姿信息；

具体地，对被测物体进行测量采用散斑相关方法或相位测量轮廓术方法，利用投影模块向待测物体表面投射结构光图像，两个相机采集到经过物体表面调制的结构光图像信息。基于散斑信息或相位信息等进行同名点查找及亚像素优化，进而实现高精度三维点云重建，重建效果如图3所示。

惯性测量单元包含三轴陀螺仪和三轴加速度计，通过惯性测量单元的输出信息，解算惯性测量单元坐标系相对于其自身基准坐标系的刚体变换，进而能够计算当前三维测量单元的位姿信息，具体地，首先通过惯性测量单元的输出信息计算惯性测量单元的相对位姿信息，进而利用惯性测量单元的相对位姿信息计算当前三维测量单元的相对位姿信息。

在本实施例中，惯性测量单元的位姿信息等同于惯性测量单元坐标系相对于其自身基准坐标系的刚体变换，即惯性测量单元其自身基准坐标系其实就是其初始各姿态角度输出为0时的坐标系。

步骤S3，保持被测物体不动，变换测量视角，利用所述三维测量单元对所述被测物体进行测量，利用惯性测量单元的输出信息解算此时惯性测量单元的位姿信息T_m'₀，保证三维测量单元在当前测量视角m与前一测量视角n获取的点云数据存在共同区域；

步骤S4，计算当前测量视角m相对前一测量视角n惯性测量单元间的位姿变换关系T_m'_n，进而计算两次测量时三维测量系统的位姿变换关系T_mn；

具体地，由图4可知，

T_m'₀＝T_n'₀T_m'_n；

故：

由于：

T_mnT_ic＝T_icT_m'_n；

故当前测量视角m相对于前一测量视角n的三维测量系统坐标系间的刚体变换可以表示为：

其中，T_n'₀指的是前一测量视角n惯性测量单元坐标系相对于初始各姿态角度输出为0时的坐标系的刚体变换，可参照附图中的图4理解，类比T_m'₀。

步骤S5，将前一测量视角的三维测量单元坐标系到全局坐标系的精确位姿变换和所述两次测量的三维测量单元的位姿变换关系应用于当前测量视角点云迭代最近点ICP配准的迭代初始值，并进行ICP迭代；

具体地，迭代初始值表示为:

T_0m0＝T_n0T_mn；

其中，T_n0为上一测量视角的ICP点云配准结果，即上一测量视角经校准后的三维测量单元的精确位姿信息；T_mn为步骤S4计算得到的刚体变换，该变换通过惯性测量单元的输出信息解算，存在较大的误差。

进一步地，ICP配准算法，使用截断符号距离函数结合光线投射的方式从预先建立的空间立方体渲染配准所需的目标点云，并且ICP配准的目标函数使用最小化点到平面距离的评价方式，即最小化以下目标函数：

其中，p_i为目标点云中的点，q_i为源点云中与p_i相对应的点，为目标点云在p_i处的单位法向量，R为待优化的旋转变换，T为待优化的平移向量。

为了提高配准效率，对待配准的点云数据进行下采样，构建多级点云数据。在ICP配准时，先把点数量较少的点云数据作为源点云进行ICP配准，并将得到的刚体变换作为下一级点云进行ICP配准的初始变换，以这种方式逐级进行迭代配准，直至到达最后一级点云，配准完成。

步骤S6，ICP迭代结束后，获得当前测量视角m与前一测量视角三维测量n系统间的精确位姿变换关系，即获得当前测量视角三维测量单元坐标系到全局坐标系的精确位姿变换关系，用它对当前惯性测量单元的位姿进行调整；

具体地，为降低惯性测量单元随时间积累的累积误差对测量的影响，当前视角的点云数据与上一视角的点云数据完成配准时，可得到两个测量视角精确的位姿变换关系，利用该变换关系对惯性测量单元的空间位置姿态进行校正，保证当前视角下惯性测量单元的输出准确，以便为下次迭代提供准确的初始位姿变换关系。

更具体地，经过ICP迭代后，步骤S5中原本存在较大误差的T_mn得到修正，即得到当前测量视角三维测量单元坐标系到全局坐标系的精确位姿变换关系T_m0，将其应用到下一测量视角的ICP配准初始值上，实现惯性测量单元的位姿的调整校正。

步骤S7，不断重复步骤S3、S4、S5、S6，直至完成对所述被测物体的测量。

步骤S8，对测量过程中获取到的点云数据进行包括全局优化、点云去重、点云滤波的优化处理，最终输出优化后的点云模型，如图5所示。

具体地，在步骤S5中，前一测量视角的三维测量单元坐标系到全局坐标系的精确位姿变换关系其实就是指上一测量视角的ICP点云配准结果，也就是步骤S6中的配准结果。但有个特殊情况，这里是一个步骤S3到S6的循环操作，首次循环时，因上一测量视角是步骤S2的中的测量(首个测量视角)，该测量视角不存在前一测量视角，其点云配准并未使用ICP配准，而是直接用惯性测量单元的输出信息解算三维测量单元到全局坐标系的刚体变换，直接使用该变换将点云变换到全局坐标系，所以此时认为的前一测量视角的三维测量单元坐标系到全局坐标系的精确位姿变换关系并非指步骤S6的配准结果，而是使用惯性测量单元输出信息计算出来的三维测量单元到全局坐标系的刚体变换。

本发明实施例提供的一种基于惯性测量单元的便携式三维测量系统及其测量方法，旨在实现灵活、稳定、低成本、高精度的三维测量。三维测量系统采用典型的双目立体视觉结构，利用散斑相关方法或相位测量轮廓术方法实现三维点云重建。在三维测量单元上固定一个惯性测量单元，通过惯性测量单元实时获取自身的相对位置，结合预先的位姿标定，就能够获得三维测量单元的位姿信息，将其应用到ICP迭代的初始值上，进而实现点云高精度配准。ICP配准完成后，利用配准结果对当前惯性测量单元的位姿进行校正，以减小惯性测量单元累积误差对测量的影响，保证下一测量视角ICP配准初始值的可靠性，提升点云配准的稳定性。

本发明实施例提供的基于惯性测量单元的便携式三维测量系统的测量方法，该测量方法将惯性测量单元固定在三维测量系统上，并标定得到惯性测量单元坐标系与三维测量单元坐标系之间的位姿变换。在测量时，使用散斑相关方法或相位测量轮廓术方法重建三维点云数据。在点云配准时，利用惯性测量单元的输出数据解算迭代最近点(ICP)算法的初始值，并进行ICP配准。由于惯性测量的累积误差随时间的积累逐渐变大，在ICP配准之后，利用配准结果对当前惯性测量单元的位姿进行校正，并将校正结果应用到下一测量视角ICP迭代的初始值上，保证下一测量视角ICP配准初值的可靠性，提升点云配准的稳定性。

本发明实施例还提供了一种终端，包括存储器、处理器及存储在存储器上且在处理器上运行的计算机程序，其特征在于，处理器执行计算机程序时，实现如图2所示的便携式三维测量系统的测量方法中的各个步骤。

本发明实施例中还提供一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如图2所示的便携式三维测量系统的测量方法中的各个步骤。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于惯性测量单元的便携式三维测量系统，其特征在于，包括三维测量单元和惯性测量单元：

所述惯性测量单元，固定在所述三维测量单元上，用于获取所述三维测量单元的自身相对位置；

所述三维测量单元，用于通过投影辅助对被测物体进行测量，获取所述被测物体表面局部的点云数据。

2.如权利要求1所述的便携式三维测量系统，其特征在于，所述三维测量单元包括两个相机和一个投影模块；

所述投影模块，用于向所述被测物体表面投射结构光图像；

所述相机，用于采集经过所述被测物体表面调制的结构光图像信息。

3.如权利要求1所述的便携式三维测量系统，其特征在于，所述惯性测量单元包括三轴陀螺仪和三轴加速度计。

4.一种如权利要求1至3任意一项所述的便携式三维测量系统的测量方法，其特征在于，包括：

步骤A，对所述便携式三维测量系统进行标定，得到所述三维测量单元和所述惯性测量单元之间的位姿变换关系，并以所述惯性测量单元各姿态角度输出为0时，所述三维测量单元的坐标系作为全局坐标系；

步骤B，利用所述三维测量单元对被测物体进行测量，得到所述被测物体表面局部的点云数据，并利用所述惯性测量单元的输出信息解算其位姿信息；

步骤C，保持所述被测物体不动，变换测量视角，利用所述三维测量单元对所述被测物体进行测量，并利用所述惯性测量单元的输出信息解算此时惯性测量单元的位姿信息，所述三维测量单元在当前测量视角和前一测量视角下获取的点云数据存在共同区域；

步骤D，计算所述惯性测量单元在当时测量视角相对于前一测量视角的位姿变换关系，并计算两次测量时所述三维测量单元的位姿变换关系；

步骤E，将前一测量视角的三维测量单元坐标系到全局坐标系的精确位姿变换关系和两次测量的三维测量单元的位姿变换关系应用于当前测量视角的点云迭代最近点ICP配准的迭代初始值，并进行迭代最近点ICP迭代；

步骤F，在迭代最近点ICP迭代结束后，得到所述三维测量单元在当时测量视角与前一测量视角之间的精确位姿变换关系，并用所述三维测量单元的精确位姿变换关系对当前所述惯性测量单元的位姿进行调整；

步骤G，重复步骤C到步骤F，直至完成对所述被测物体的测量；

步骤H，对测量过程中的获取的点云数据进行优化处理，得到点云模型。

5.如权利要求4所述的测量方法，其特征在于，所述步骤A中，对所述便携式三维测量系统进行标定包括对两个相机的标定和对所述惯性测量单元位姿的标定，其中：

对两个相机的标定包括：获取两个相机的内部参数、镜头畸变参数及两个相机坐标系间的刚体变换；

对所述惯性测量单元位姿的标定包括：计算所述惯性测量单元的坐标系相对于三维测量单元的坐标系的刚体变换T_ic,其中，R_ic表示旋转变换，t_ic表示平移向量。

6.如权利要求4所述的测量方法，其特征在于，所述利用所述三维测量单元对被测物体进行测量，得到所述被测物体表面局部的点云数据包括：

采用散斑相关方法，利用所述三维测量单元向所述被测物体的表面投射结构光图像，两个相机采集经过所述被测物体的表面调制的结构光图像信息，利用所述结构光图像信息中的散斑信息进行同名点查找及亚像素优化，得到所述被测物体表面局部的点云数据；

或：

采用相位测量轮廓术方法，利用所述三维测量单元向所述被测物体的表面投射结构光图像，两个相机采集经过所述被测物体的表面调制的结构光图像信息，利用所述结构光图像信息中的相位信息进行同名点查找及亚像素优化，得到所述被测物体表面局部的点云数据。

7.如权利要求4所述的测量方法，其特征在于，所述利用所述惯性测量单元的输出信息解算其位姿信息包括：

通过所述惯性测量单元的输出信息，解算惯性测量单元坐标系相对于其自身基准坐标系的刚体变换，根据所述惯性测量单元坐标系相对于其基准坐标系的刚体变换计算当前所述三维测量单元的位姿信息。

8.如权利要求4所述的测量方法，其特征在于，此时惯性测量单元的位姿信息以T′_m0表示，当前测量视角以m表示，前一测量视角以n表示，所述惯性测量单元在当时测量视角相对于前一测量视角的位姿变换关系以T′_mn表示，两次测量时所述三维测量单元的位姿变换关系以T_mn表示；

根据T′_m0＝T′_n0T′_mn，得到其中T′_n0是前一测量视角n的位姿信息；

根据T_mnT_ic＝T_icT′_mn，得到当前测量视角m相对于前一测量视角n的三维测量单元坐标系间的刚体变换T_mn，则

9.如权利要求8所述的测量方法，其特征在于，迭代初始值以T_0m0表示，T_0m0＝T_n0T_mn，其中，T_n0为上一测量视角的ICP点云配准结果，步骤E具体包括：

使用截断符号距离函数结合光线投射的方式从预先建立的空间立方体渲染配准所需的目标点云；

最小化目标函数f(R,T)，即：

其中，p_i为目标点云中的点，q_i为源点云中与p_i相对应的点，为目标点云在p_i处的单位法向量，R为待优化的旋转变换，T为待优化的平移向量；

对待配准的点云数据进行下采样，构建多级点云数据；

将点数量较少的点云数据作为源点云进行ICP配准，并将得到的刚体变换作为下一级点云进行ICP配准的初始变换，以这种方式逐级进行迭代配准，直至到达最后一级点云，完成配准。

10.如权利要求8所述的测量方法，其特征在于，所述用所述三维测量单元的精确位姿变换关系对当前所述惯性测量单元的位姿进行调整包括：

将所述三维测量单元的精确位姿变换关系应用到下一测量视角的ICP配准初始值上，以实现对惯性测量单元进行位姿调整。