[发明专利]基于惯性测量单元与RGB-D传感器的三维重建方法

权利要求书

1.一种基于惯性测量单元与RGB-D传感器的三维重建方法，其特征在于包括以下步骤：

步骤一、采用SURF算法进行关键点的提取，接下来采用基于局部表面拟合的方法计算各关键点处法向量，然后计算关键点的三维快速点特征直方图FPFH描述子

FPFH(Pq)=SPFH(Pq)+1kΣi=1k1ωk·SPFH(Pk)---(1)]]>

其中，P_q表示查询点，P_k表示其邻近点，权重ω_k表示查询点P_q和其邻近点P_k之间的距离；SPFH简化的点特征直方图的计算是依据这个点和它的K邻域点之间的一个元组得到的；

步骤二、通过三轴陀螺仪和三轴加速度计获取一个粗略变换矩阵；首先获得三轴陀螺仪和三轴加速度计所测量RGB-D传感器的XYZ三轴向的位移变化x、y和z以及三轴向的夹角变化α、β和γ；求得旋转矩阵R＝R_x R_y R_z，其中R_x、R_y以及R_z分别为传感器绕X轴、Y轴和Z轴的旋转矩阵

Rx=1000cosα-sinα0sinαcosαRy=cosβ0sinβ010-sinβ0cosβRz=cosγ-sinγ0sinγcosγ0001---(2)]]>

定义一个4阶的变换矩阵T如下：

T=xRyz0001---(3)]]>

其次通过kd-tree完成待配准点云集关键点与参考点云集关键点快速点特征直方图FPFH描述子的特征匹配；进行M次随机选点对

M=log(1-p)log(1-(1-ϵ)s)---(4)]]>

其中，s为最少要选取点对的个数，ε为局内点的百分比，p是所需的匹配成功概率；通过每次随机选取的点对完成一个变换估计，接着通过扩展卡尔曼滤波算法将此变换与通过惯性测量单元所得到的变换T进行数据融合，得到一个融合后的变换T_EKF；使用T_EKF对参考点云集中的关键点进行坐标变换，并计算每一对匹配关键对之间的欧式距离

Disij=[Σk=0k=n(Pik-Pjk)2]1/2---(5)]]>

其中，P为点云集中的点，Dis为点对间的距离；将距离Dis小于一定阈值的点对视为局内点对，剩余的点对视为局外点对，选取局内点对数最多的一次进行后续计算，通过局内点对进行N次估计，粗略估计变换T_c；

最终将获得的粗略估计变换T_c与通过惯性测量单元所得到的变换T作用于其中一点云集，计算与另一点云集间的最小均方根，取使其中值较小的变换作为新的变换T_c；

步骤三、将T_c作为ICP算法的初始值，避免算法局部收敛；将粗略变换T_c作用于参考点云集关键点获得一个新的位置的参考关键点云集，其次利用ICP算法精确估计该参考关键点到待匹配关键点的坐标变换，方法如下：

首先进行错误匹配点对的去除，然后通过剩余关键点估计坐标变换T_i；对以上几步进行迭代直到满足任意以下条件时停止：

1)迭代到达一定的次数N；

2)本次计算得到的精确坐标变换T_i与上次迭代得到的坐标变换T_i^*之间的变化小于某一阈值；

3)其均方根误差E(a)小于某一阈值

E(a)=Σi=1Ndωiminjϵ2(|mj-T(ak;di)|),---(6)]]>

式中，m_j和d_i分别为不同点云集中的点；

最终得到精确坐标变换估计T＝T_iT_c，利用该坐标变换完成对两个三维点云集的拼接；

步骤四、当检测到Loop Closure时，通过ELCH算法快速对全局构图进行优化；

计算相邻两节点v_i与v_j间边的协方差c_i,j，通过ICP算法计算对Loop Closure检测到的两个节点进行处理得到一个坐标变换ΔX；然后通过如下方法将权重在每一个节点对之间进行分配：

wi=ws+d(vs,vi)d(vs,ve)(we-ws)---(7)]]>

其中，w_s与w_e为环路开始与结束时的两个节点v_s与v_e的权重，w_i为环路间节点的权重；d(v_l,v_k)表示从节点vl到vk所经过边的协方差c_i,j的求和

d(vl,vk):=Σedge{i,j}∈Pathvltovkcij---(8)]]>

最终通过以上权重分配完成对全局地图的初步优化后通过LUM算法求出最优解。