技术领域
[0001] 本
发明涉及室内定位技术领域，尤其涉及一种无人机室内定位方法及系统。
背景技术
[0002] 近十多年来，为解决定位“最后一公里”的问题，各科研机构以及创业型公司等在室内定位方面开展了大量研究，旨在找到一套良好的室内定位解决方案。
[0003] 目前，存在的定位系统主要有四种：
[0004] 第一种：基于GPS或北斗等的卫星定位系统。但其主要应用的室外定位，不适用于室内定位，且其定位误差较大。
[0005] 第二种：基于WIFI的定位。其便利性强，但由于
信号在传播中会收到折射、反射、非视距因素、信号死
角及2.4GHz等高频段干扰，定位误差较大。
[0006] 第三种：基于蓝牙的定位。其基于三角定位法，定位精准，但需要蓝牙模
块及部署蓝牙基站，导致室内定位成本较高。
[0007] 第四种：超宽带室内定位系统。其采用超宽带脉冲信号，通过预设的
传感器对信号标签进行追踪分析，多径分辨能
力强，
精度高，但该类定位系统需要在室内布置用于定位的传感器，成本高，定位范围有限，可移植性差。
[0008] 综上所述，现有的室内定位技术在定位精度、成本、
稳定性、可移植等方面还无法兼顾，造成室内定位技术发展缓慢。
[0009] 近些年，随着无人机技术的快速发展，无人机的应用领域也得到极大拓展。目前，无人机定点定位飞行控制方面主要依赖传统的GPS或第三方定位传感器。基于前文对室内定位技术现状的分析可知，无人机在室内等实时定位飞行控制要求较高的地方仍然受到很大技术局限性，其面临的主要技术问题包括以下几个方面：
[0010] 1、缺乏可用于准确判断
位置信息的可依赖特征。
[0011] 2、缺乏可用于准确估计位置信息和追踪位置变化的方法。
[0012] 3、不能准确估计无人机空间六
自由度定位信息。
发明内容
[0013] 本发明所要解决的技术问题是，提供一种无人机室内定位方法及系统，实现对无人机进行室内实时定位。本发明是这样实现的：
[0014] 一种无人机室内定位方法，所述无人机上安装有深度传感器；所述定位方法包括如下步骤：
[0015] 步骤A：利用所述深度传感器按设定采集
频率实时采集室内3D点
云图像，并检测其中的平面；
[0016] 步骤B：实时计算各
帧3D点云图像中的从各平面中预选出的预设数量的配准参考平面的法向量和
重心；各配准参考平面中的一个为定位基准平面；
[0017] 步骤C：实时根据当前帧与上一帧3D点云图像中各配准参考平面的法向量和重心计算所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量；
[0018] 步骤D：实时根据所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵计算所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的
姿态角度变化量；
[0019] 步骤E：实时根据上一帧3D点云图像中的所述定位基准平面的法向量和重心及所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量和平移向量计算在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的姿态角度变化量和平移向量；
[0020] 步骤F：根据实时计算得出的在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的姿态角度变化量和平移向量控制所述无人机向所述定位基准平面的重心飞行；
[0021] 步骤G：当计算得出在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的平移向量小于预设的第一
阈值时，停止控制所述无人机向所述定位基准平面的重心飞行，定位结束。
[0022] 进一步地，所述检测其中的平面的步骤包括：
[0023] 将所述3D点云图像划分为若干点云单元；
[0024] 计算各点云单元的
最小均方误差,同时，剔除符合预设剔除条件的点云单元；
[0025] 建立基于剩余各点云单元的最小均方误差的二叉堆排序，并选择其中最小均方误差最小的点云单元作为膨胀起始
节点；
[0026] 从所述膨胀起始节点开始进行膨胀生长以产生所述3D点云图像中的各平面。
[0027] 进一步地，所述符合预设剔除条件的点云单元包括如下至少一种：
[0028] 最小均方误差大于预设的第二阈值的点云单元；
[0029] 数据有丢失的点云单元；
[0030] 深度数据不连续的点云单元。
[0031] 进一步地，所述配准参考平面的法向量和重心的计算方法如下：
[0032] 提取各配准参考平面中的各点云单元进行主成分分析，并根据分析结果计算各配准参考平面的法向量和重心。
[0033] 进一步地，所述步骤C包括：
[0034] 根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心分别计算所述旋转矩阵和平移向量；
[0035] 根据各配准参考平面的大小为根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心计算出的所述旋转矩阵和平移向量设定相应的权重；配准参考平面越大，根据其在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量的权重越高；
[0036] 根据计算出的各旋转矩阵和平移向量的权重对计算出的各旋转矩阵和平移向量进行加权求和，并将得到的最终的旋转矩阵和平移向量作为所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量。
[0037] 一种无人机室内定位系统，所述无人机上安装有深度传感器；所述定位系统包括：
[0038] 平面检测模块，用于检测利用所述深度传感器按设定采集频率实时采集的室内3D点云图像中的平面；
[0039] 3D特征检测模块，用于实时计算各帧3D点云图像中的从各平面中预选出的预设数量的配准参考平面的法向量和重心；各配准参考平面中的一个为定位基准平面；
[0040] 帧间移动变量计算模块，用于实时根据当前帧与上一帧3D点云图像中各配准参考平面的法向量和重心计算所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量；
[0041] 帧间姿态角度变化量计算模块，用于实时根据所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵计算所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量；
[0042] 飞行控制参数计算模块，用于实时根据上一帧3D点云图像中的所述定位基准平面的法向量和重心及所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量和平移向量计算在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的姿态角度变化量和平移向量；
[0043] 飞行控
制模块，用于根据实时计算得出的在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的姿态角度变化量和平移向量控制所述无人机向所述定位基准平面的重心飞行；
[0044] 所述飞行
控制模块还用于在当计算得出在当前帧3D点云图像中要将所述深度传感器定位到所述定位基准平面的重心所需要的平移向量小于预设的第一阈值时，停止控制所述无人机向所述定位基准平面的重心飞行，定位结束。
[0045] 进一步地，所述平面检测模块具体用于：
[0046] 将所述3D点云图像划分为若干点云单元；
[0047] 计算各点云单元的最小均方误差,同时，剔除符合预设剔除条件的点云单元；
[0048] 建立基于剩余各点云单元的最小均方误差的二叉堆排序，并选择其中最小均方误差最小的点云单元作为膨胀起始节点；
[0049] 从所述膨胀起始节点开始进行膨胀生长以产生所述3D点云图像中的各平面。
[0050] 进一步地，所述符合预设剔除条件的点云单元包括如下至少一种：
[0051] 最小均方误差大于预设的第二阈值的点云单元；
[0052] 数据有丢失的点云单元；
[0053] 深度数据不连续的点云单元。
[0054] 进一步地，所述3D特征检测模块具体用于：
[0055] 提取各配准参考平面中的各点云单元进行主成分分析，并根据分析结果计算各配准参考平面的法向量和重心。
[0056] 进一步地，所述帧间移动变量计算模块具体用于：
[0057] 根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心分别计算所述旋转矩阵和平移向量；
[0058] 根据各配准参考平面的大小为根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心计算出的所述旋转矩阵和平移向量设定相应的权重；配准参考平面越大，根据其在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量的权重越高；
[0059] 根据计算出的各旋转矩阵和平移向量的权重对计算出的各旋转矩阵和平移向量进行加权求和，并将得到的最终的旋转矩阵和平移向量作为所述深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量。
[0060] 与
现有技术相比，本发明通过安装在无人机上的深度传感器实时采集室内3D点云图像并检测其中的平面，并利用平面的法向量和重心作为定位的依赖特征，相比依赖点的三维特征进行定位稳定性更高，可准确判断出无人机的位置信息。同时，本发明通过帧间各配准参考平面的法向量及重心实时计算帧间深度传感器的位置变化信息，可实时准确追踪无人机的位置变化情况，实现对无人机的精确飞行控制。
附图说明
[0061] 图1：本发明无人机室内定位方法流程示意图；
[0063] 图3：本发明无人机室内定位系统流程示意图。
具体实施方式
[0064] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及
实施例，对本发明进行进一步详细说明。本发明的基本思想是，通过安装在无人机上的深度传感器实时采集室内3D点云图像，并根据3D点云图像中的平面的3D特征对无人机进行定位，在定位的过程中，通过相邻帧3D点云图像中的对应平面的3D特征相邻帧之间无人机的位置变化量，并根据该位置变化量来实时追踪无人机的位置变化情况，从而控制无人机飞行，实现定位。结合图1，本发明提供的无人机室内定位方法包括如下步骤：
[0065] 步骤A：利用深度传感器按设定采集频率实时采集室内3D点云图像，并检测其中的平面。本发明利用Xtion深度传感器按不低于设定值的频率实时采集室内3D点云图像，该3D点云图像为有序3D点云数据。针对Xtion深度传感器实时采集到的各帧3D点云图像，再检测其中的平面。检测3D点云图像中的平面主要包括以下⑴、⑵、⑶、⑷四个步骤：
[0066] ⑴、将3D点云图像划分为若干点云单元。将3D点云图像划分为若干点云单元可提升后续聚类生长阶段的处理速度。例如，一帧640*480的有序3D点云数据，其有307200个深度坐标数据，可划分为3072个点云单元，每个点云单元为10*10
分辨率，包含100个深度坐标数据。通过这样的方式将3D点云图像进行分块处理比对其进行整体
数据处理的效率要高。
[0067] ⑵、计算各点云单元的最小均方误差，同时，剔除符合预设剔除条件的点云单元。点云单元的最小均方误差的计算方法如下：
[0068] ①首先确定该点云单元的最佳拟合平面，步骤如下：
[0069] ⒈对点云单元求解其质心，其质心定义为 其中，m为点云单元的重心，n为点云单元中所包含的三维点的数量，i为点云单元中三维点的序号，pi为点云单元中第i个三维点的三维坐标。
[0070] ⒉拟合该点云单元的特征矩阵：其中，n为点云单元中所包含的三维点的数量，φi为节点
单元中第i个φ坐标，mφ为φ坐标的重心。
[0071] ⒊计算上述特征矩阵的特征值和
特征向量。该点云单元的最佳拟合平面的法向量就等于该上述特征矩阵的最小特征值所对应的特征向量。特征向量经过归一化处理后，可求偏移值bias：
[0072] bias＝-(nx·mx+ny·my+nz·mz)，其中，mx,my,mz为重心三维坐标，nx,ny,nz平面法线坐标。
[0073] ⒋拟合出的该点云单元的最佳拟合平面方程为：nx·x+ny·y+nz·z+bias＝0，其中，。点m为最佳平面的质心，其通过法向量和偏移值bias共同确定最佳拟合平面的方程。
[0074] ②根据最佳拟合平面方程求解最小均方误差：
[0075]
[0076] 注意在上述求解点云单元的最小均方误差的过程中，需要剔除不符合要求的点云单元，包括以下几类：
[0077] 第一类：有较大最小均方误差的点云单元。这类点云单元含有不在同一个平面的点的干扰，可设定一阈值TMSE，将最小均方误差大于该阈值的点云单元剔除。TMSE＝2 2 -6
(σz+ε)，其中σ＝1.6×10 ，3≤ε≤8，ε可以根据需要选择，z为深度距离。
[0078] 第二类：数据有丢失的点云单元。一般是指这类点云单元中有没有采集到深度坐标数据的点。这类点云单元深度数据有丢失，影响对平面的检测。
[0079] 第三类：深度数据不连续的点云单元。当该点云单元中的各深度数据之间满足|Za-Zb|＞2α(|Za|+0.5)时，表示该点云单元中含有不在同一平面的三维点。其中，Za、Zb表示深度距离值。这类点云单元一般位于平面之间的边缘处，用于区分不同平面的分界线，也会干扰对平面的检测。
[0080] 通过上述步骤⑴、⑵对3D点云图像完成了粗分割处理，并初步建立起了各点云单元之间的关联，将各类不符合条件的点云单元剔除后，将属于同一个平面的点云单元之间事先建立了连接关系，不属于同一个平面的点云单元之间则没有连接关系。步骤⑶、⑷是对各点云单元进行聚类生长算法处理以从剩下的各点云单元中生长出各平面的过程：
[0081] ⑶、建立基于剩余各点云单元的最小均方误差的二叉堆排序，并选择其中最小均方误差最小的点云单元作为膨胀起始节点。
[0082] ⑷、从膨胀起始节点开始进行膨胀生长以产生3D点云图像中的各平面。膨胀过程如图2所示，图2中，a表示一点云单元，b、c、d、e分别表示点云单元a的上、左、下、右邻居点云单元。根据事先已经建立起的连接关系进行膨胀生长，如果一个点云单元存在上、左、下、右邻居点云单元，则将该点云单元的上、左、下、右邻居点云单元与该点云单元合并为一个集合，然后检测新合并到该集合的点云单元是否存在上、左、下、右邻居点云单元，如果有，也将其上、左、下、右点云单元合并到该集合，通过不断的合并，将所有符合要求的点云单元都合并到该集合后，该集合就是该3D点云图像中的一个平面。然后按照前述同样的方法确定下一个平面，直到检测完3D点云图像中的所有平面。
[0083] 步骤B：实时计算各帧3D点云图像中的从各平面中预选出的预设数量的配准参考平面的法向量和重心，各配准参考平面中的一个为定位基准平面。配准参考平面是指用于根据其在相邻两帧3D点云图像中的3D特征的变化量来确定深度传感器在拍摄该相邻两帧3D点云图像之间的位置变化量的平面。而通过多个这样的配准参考平面综合计算可进一步提高计算出的上述位置变化量的准确性。由于平面越大，其3D特征越稳定，利用其计算上述位置变化量的准确性越高，因此，可根据步骤A检测到的各平面的大小来预选一定数量的配准参考平面。可将步骤A检测到的各平面按大小进行排序，选择最大的若干个平面作为配准参考平面。可根据需要确定配准参考平面的数量。在从选择的各配准参考平面中选择一个作为定位基准平面。本发明定位的目的就是要将无人机定位到该定位基准平面的重心的一定范围内。
[0084] 配准参考平面的法向量和重心的计算方法如下：
[0085] 提取配准参考平面中的各点云单元并对其进行主成分分析(Principal Component Analysis，PCA)，并根据分析结果计算配准参考平面的法向量和重心。例如，如果某一配准参考平面中包含三个点云单元，对该三个点云单元进行PCA分析得到该三个点云单元的法线分别为：N1(X1,Y1,Z1)、N2(X2,Y2,Z2)、N3(X3,Y3,Z3)，则该配准参考平面的法线为：
[0086]其 中，S 1＝X 12+Y12+Z12，
2 2 2 2 2 2
S2＝X2+Y2+Z2，S3＝X3+Y3+Z3。其他情
况同理。
[0087] 步骤C：实时根据当前帧与上一帧3D点云图像中各配准参考平面的法向量和重心计算深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量。步骤C是将两个
坐标系中的数据配准到同一坐标系的过程。该旋转矩阵和平移向量实质表示了深度传感器在拍摄上一帧3D点云图像与拍摄当前帧3D点云图像之间的位置变化量。配准参考平面的法向量和重心的数据是以拍摄该配准参考平面时深度传感器的坐标系为
基础的。如前述，由于无人机在连续飞行，因此，深度传感器在拍摄上一帧3D点云图像时与拍摄当前帧3D点云图像时的视角是不同的，即上一帧与当前帧3D点云图像中的配准参考平面的法向量和重心是以不同的坐标系为基础的，因此，需要将上一帧与当前帧3D点云图像中的配准参考平面的坐标数据统一到同一坐标系下，以实现对深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量的计算。配准时，最简单且常用的几何特征是点，但是，由于点的不稳定性会带来很大的误差。前述已通过聚类生长算法提取的平面稳定且准确，而且平面的对应关系较好，具有很好的鲁棒性，上下帧3D点云图像中的平面的变化极其微小。因此选择基于平面特征实现配准。步骤C具体包括如下⑴、⑵、⑶三步骤：
[0088] ⑴、根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心分别计算旋转矩阵和平移向量。
[0089] ⑵、根据各配准参考平面的大小为根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量设定相应的权重，配准参考平面越大，根据其在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量的权重越高。
[0090] ⑶、根据计算出的各旋转矩阵和平移向量的权重对计算出的各旋转矩阵和平移向量进行加权求和，并将得到的最终的旋转矩阵和平移向量作为深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量。以下为计算该旋转矩阵和平移向量需要用到的公式。
[0091] 其中，wi为权重，ni和n'i为上下帧对应平面的法线，R即为要解出的旋转矩阵。可利用四元数法求出R。
[0092] 其中，wi为权重，ni和n'i为上下帧对应平面的法线，R即为要解出的旋转矩阵，mi和m'i上下帧中对应平面的重心，，将求出的旋转矩阵设为R，通过对该公式进行最小化运算获得t，并对t求导即可求得平移向量 在计算过程中，至少需要3对配准参考平面(即配准参考平面的数量为3个，上一帧与当前帧3D点云图像中的
3个配准参考平面组成3对配准参考平面)才能得到唯一的旋转矩阵和平移向量，本发明选择5对配准参考平面会出现冗余的平面特征对，可利用最小二乘法进行优化。
[0093] 步骤D：实时根据深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵计算深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量。根据深度传感器的旋转矩阵可以求出深度传感器的姿态角度变化量。姿态角度变化量包括翻滚角度变化量α、
俯仰角度变化量β以及航向角度变化量γ，它们与旋转矩阵R的坐标关系如下：
[0094] 其中，，，
[0095] 由此，可计算姿态角度表达式：
[0096]
[0097] 步骤E：实时根据上一帧3D点云图像中的定位基准平面的法向量和重心及深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量和平移向量计算在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的姿态角度变化量和平移向量。根据上一帧3D点云图像中的定位基准平面的法向量和重心可以求出在上一帧3D点云图像中要将深度传感器定位到上述定位基准平面的重心所需要的姿态角度变化量和平移向量，再根据深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量就可计算出在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的姿态角度变化量。
[0098] 步骤F：根据实时计算得出的在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的姿态角度变化量和平移向量控制无人机向定位基准平面的重心飞行。
[0099] 步骤G：当计算得出在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的平移向量小于预设的第一阈值时，停止控制无人机向定位基准平面的重心飞行，定位结束。由于重心位于定位基准平面上，因此，为避免无人机与定位基准平面相撞，不能真正将最终的目标位置设为重心，当上述所需要的平移向量小于预设的第一阈值时，就可认为已经达到目标位置，使无人机停止继续朝定位基准平面的重心飞行。
[0100] 基于上述无人机室内定位方法，本发明还提出了一种无人机室内定位系统，无人机上安装有深度传感器。该定位系统包括：
[0101] 平面检测模块1，用于检测利用深度传感器按设定采集频率实时采集的室内3D点云图像中的平面；
[0102] 3D特征检测模块2，用于实时计算各帧3D点云图像中的从各平面中预选出的预设数量的配准参考平面的法向量和重心，各配准参考平面中的一个为定位基准平面；
[0103] 帧间移动变量计算模块3，用于实时根据当前帧与上一帧3D点云图像中各配准参考平面的法向量和重心计算深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量；
[0104] 帧间姿态角度变化量计算模块4，用于实时根据深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵计算深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量；
[0105] 飞行控制参数计算模块5，用于实时根据上一帧3D点云图像中的定位基准平面的法向量和重心及深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的姿态角度变化量和平移向量计算在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的姿态角度变化量和平移向量；
[0106] 飞行控制模块6，用于根据实时计算得出的在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的姿态角度变化量和平移向量控制无人机向定位基准平面的重心飞行；
[0107] 飞行控制模块6还用于在当计算得出在当前帧3D点云图像中要将深度传感器定位到定位基准平面的重心所需要的平移向量小于预设的第一阈值时，停止控制无人机向定位基准平面的重心飞行，定位结束。
[0108] 平面检测模块1具体用于：
[0109] 将3D点云图像划分为若干点云单元；
[0110] 计算各点云单元的最小均方误差,同时，剔除符合预设剔除条件的点云单元；
[0111] 建立基于剩余各点云单元的最小均方误差的二叉堆排序，并选择其中最小均方误差最小的点云单元作为膨胀起始节点；
[0112] 从膨胀起始节点开始进行膨胀生长以产生3D点云图像中的各平面。
[0113] 符合预设剔除条件的点云单元包括如下至少一种：
[0114] 最小均方误差大于预设的第二阈值的点云单元；
[0115] 数据有丢失的点云单元；
[0116] 深度数据不连续的点云单元。
[0117] 3D特征检测模块2具体用于：
[0118] 提取各配准参考平面中的各点云单元进行主成分分析，并根据分析结果计算各配准参考平面的法向量和重心。
[0119] 帧间移动变量计算模块3具体用于：
[0120] 根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心分别计算旋转矩阵和平移向量；
[0121] 根据各配准参考平面的大小为根据每一配准参考平面在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量设定相应的权重，配准参考平面越大，根据其在当前帧与上一帧3D点云图像中的法向量和重心计算出的旋转矩阵和平移向量的权重越高；
[0122] 根据计算出的各旋转矩阵和平移向量的权重对计算出的各旋转矩阵和平移向量进行加权求和，并将得到的最终的旋转矩阵和平移向量作为深度传感器从拍摄上一帧3D点云图像时到拍摄当前帧3D点云图像时的旋转矩阵和平移向量。
[0123] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何
修改、等同替换和改进等，均应包含在本发明的保护范围之内。
