CN110503688A

CN110503688A - 一种用于深度相机的位姿估计方法

Info

Publication number: CN110503688A
Application number: CN201910769449.7A
Authority: CN
Inventors: 朱俊涛; 陈强
Original assignee: Shanghai University of Engineering Science
Current assignee: Shanghai University of Engineering Science
Priority date: 2019-08-20
Filing date: 2019-08-20
Publication date: 2019-11-26
Anticipated expiration: 2039-08-20
Also published as: CN110503688B

Abstract

本发明涉及定点跟踪的技术领域，公开了一种用于深度相机的位姿估计方法，包括将深度相机设置在移动机构上进行拍摄，获得深度图和RBG彩色图，从拍摄的每相邻两帧图像中提取ORB特征点对；利用N个深度信息缺失的ORB特征点对，计算相机位姿变化的估计值ξ_P，利用M个深度信息完整的ORB特征点对，计算相机位姿变化的估计值ξ_Q，进而得到总估计值ξ₀；构建融合了深度信息缺失的ORB特征点对和深度信息完整的ORB特征点对对应的误差信息的最小重投影误差模型，进而得到对应的雅可比矩阵J；根据总估计值ξ₀、最小重投影误差函数和雅可比矩阵J，利用非线性优化方法，计算优化后的总估计值ξ_k，从而完成相邻两帧图像拍摄过程中的相机位姿变化的估计。

Description

一种用于深度相机的位姿估计方法

技术领域

本发明涉及定位追踪的技术领域，具体涉及一种用于深度相机的位姿估计方法。

背景技术

现有市面上的Kinect相机所捕获的深度图像由于遮挡、吸收、散斑、反射等原因导致深度区域缺失经常产生缺失深度区域的现象，使得传统ICP算法迭代相机位姿时，有时出现特征点丢失导致算法无法收敛或误差过大等问题，进行点云配准时出现了大量误差点，导致其匹配点对减少，在初始位姿选取时，可能偏离迭代区间，使得迭代次数增加，收敛速度慢，收敛成功率低。

对于Kinect的深度缺失问题上有许多研究方法，其中最为常见的方法是结合彩色图像纹理对深度图像进行修复，如(Le A V,Jung SW,Won C S.Directional jointbilateral filter for depth images.[J].Sensors,2014,14(7):11362.)或者(MatsuoT,Fukushima N,Ishibashi Y.Weighted jointbilateral filter with slope depthcompensation filter for depth ma prefinement[C].International Conference onComputer Vision Theory andApplications.2015.)。此类方法虽然可以恢复出完整的深度图，但是彩色图像的边缘和深度图像的边缘不能基本匹配且误差较大，且修复时间过长，优化后精度提高不明显，不适合机器人的实时作业。

发明内容

本发明提供了一种用于深度相机的位姿估计方法，解决了现有方法对深度信息缺失的修复时间过长，优化后精度提高不明显，不适合机器人的实时作业等问题。

本发明可通过以下技术方案实现：

一种用于深度相机的位姿估计方法，包括以下步骤：

步骤一、将深度相机设置在移动机构上进行拍摄，获得深度图和RBG彩色图，从拍摄的每相邻两帧图像中提取Oriented FAST and Rotated BRIEF ORB特征点对；

步骤二、利用N个深度信息缺失的ORB特征点对，计算相机位姿变化的估计值ξ_P，利用M个深度信息完整的ORB特征点对，计算相机位姿变化的估计值ξ_Q，进而得到总估计值ξ₀；

步骤三、构建融合了深度信息缺失的ORB特征点对和深度信息完整的ORB特征点对对应的误差信息的最小重投影误差模型，进而得到对应的雅可比矩阵J；

步骤四、根据所述总估计值ξ₀、最小重投影误差模型和雅可比矩阵J，利用非线性优化方法，计算优化后的总估计值ξ_k，从而完成相邻两帧图像拍摄过程中的相机位姿变化的估计。

进一步，对于N个深度信息缺失的ORB特征点对，利用重投影方法，结合PnP算法，构建其对应的误差函数e_P；对于M个深度信息完整的ORB特征点对，利用重投影方法，结合ICP算法，构建其对应的误差函数e_Q；构建融合误差函数e_P和误差函数e_Q的最小重投影误差模型e，进而得到对应的雅可比矩阵J。

进一步，所述雅可比矩阵J的计算方法包括以下步骤：

步骤Ⅰ、对于N个深度信息缺失的ORB特征点对，记相邻两帧图像上匹配的ORB特征点对对应的空间点齐次坐标分别为P_i＝(x_i,y_i,z_i,1)^T，其相邻帧上的ORB特征点经过世界坐标系和像素坐标系的转换关系得到对应的像素坐标为

利用重投影方法，计算初始帧的ORB特征点P_i＝(x_i,y_i,z_i,1)^T在相邻帧上投影的像素齐次坐标p_i＝(u_i,v_i,1)^T，其李代数关系如下，

其中，si表示初始帧的ORB特征点在相邻帧上的投影对应的深度信息，K表示相机的内参矩阵，^表示反对称符号，

结合PnP算法，构建深度信息缺失的ORB特征点对的误差函数；

步骤Ⅱ、对于M个深度信息完整的ORB特征点对，记相邻两帧图像上匹配的ORB特征点对对应的空间点坐标分别为Q_i＝(X_i,Y_i,Z_i)^T，利用重投影方法，计算初始帧的ORB特征点Q_i＝(X_i,Y_i,Z_i)^T在相邻帧上投影的空间点坐标其李代数关系如下，

其中，^表示反对称符号，

结合ICP算法，构建深度信息完整的ORB特征点对的误差函数；

步骤Ⅲ、构建最小重投影误差模型e，如下所示；

其中，δξ表示扰动量，f_x表示相机的焦距在像素坐标系的x轴分量，f_y表示相机的焦距在像素坐标系的y轴分量，点P_i′＝(x′_i,y′_i,z′_i,1)^T表示初始帧中深度信息缺失的ORB特征点P_i＝(x_i,y_i,z_i,1)^T乘以相机位姿变化的总估计值ξ₀后对应的点，点Q_i′＝(X′_i,Y_i′,Z′_i)^T表示初始帧中深度信息完整的ORB特征点Q_i＝(X_i,Y_i,Z_i)^T乘以相机位姿变化的总估计值ξ₀后对应的点。

进一步，所述非线性优化方法设置为高斯牛顿法。

进一步，利用高斯牛顿法，计算优化后的总估计值ξ_k的方法包括以下步骤：

1)给定总估计值ξ₀；

2)对于第k次迭代，求出当前的雅克比矩阵J与误差e；

3)求解增量Δξ_k＝g/H，其中，H＝J^TJ,g＝-J^Te(ξ)；

4)若Δξ_k小于阈值，则停止，并输出优化后的总估计值ξ_k；否则，令ξ_k+1＝ξ_k+Δξ_k，返回第2)步。

进一步，对于N个深度信息缺失的ORB特征点对，利用RANSAC算法计算估计值ξ_P；对于M个深度信息完整的ORB特征点对，利用直接线性变换法计算估计值ξ_Q，再利用如下方程式，计算总估计值ξ₀。

进一步，所述深度相机拍摄的每相邻两帧图像都有重叠区域，所述ORB特征点从重叠区域提取。

本发明有益的技术效果在于：

通过对深度信息缺失和完整的ORB特征点对分别计算相机位姿变化的估计值，再将两者进行加权融合成总估计值，然后，构建融合了深度信息缺失和完整的ORB特征点对对应的误差信息的最小重投影误差模型，在此基础上得到对应的雅可比矩阵，最后，利用非线性优化方法如高斯牛顿法进行迭代优化，求解优化后的总估计值，从而大大增加了用于计算的匹配成功的特征点对数量，提高了位姿变化的估计值的计算精度，同时大大减少了迭代次数，提高了非线性优化方法的收敛成功率，提高了里程计的鲁棒性；较传统ICP算法具有优化速度较快的优势，整个里程计算法一般为10ms左右，其优化精度不亚于传统花费大量效率进行深度修复后再迭代优化的精度，具有高效的实时性，提高了对移动机构的控制精度，满足其运动控制的实时性要求。另外，本发明简单可靠，操作方便，易于实现，便于推广应用。

附图说明

图1为本发明的流程示意图；

图2为采用不同控制方法对移动机器人的运动轨迹进行估计的对比示意图，其中，x轴表示运动距离，y轴表示误差。

具体实施方式

下面结合附图及较佳实施例详细说明本发明的具体实施方式。

如图1所示，本发明的流程示意图，本发明提供了一种用于深度相机的位姿估计方法，主要是针对相邻的两帧图像，将深度信息缺失的ORB特征点对和深度信息完整的ORB特征点对分开处理，计算对应的相机位姿变化的估计值，进而得到总估计值，再利用融合了两者对应的误差信息的最小重投影误差模型，得到对应的雅可比矩阵J，最后利用非线性优化方法，计算优化后的总估计值ξ_k，完成相邻两帧图像拍摄过程中的相机位姿变化的估计。具体包括以下步骤：

步骤一、将深度相机设置在移动机构上，对当前场景进行拍摄，获得深度图和RBG彩色图，从拍摄的每相邻两帧图像中提取匹配好的Oriented FAST and Rotated BRIEFORB特征点对。其ORB特征点的提取方法可以参照由Rublee E,Rabaud V,Konolige K,etal.ORB:An efficient alternativeto SIFT or SURF[C]//IEEE InternationalConference on Computer Vision.Piscataway,USA:IEEE,2011:2564-2571.所公开的方法进行。

为了确保计算的精度和速度，当前场景的光线不要太强，相邻的两帧图像必须有重叠区域，不一定是相邻的上一帧和下一帧，只要两帧之间有足够的重叠区域即可，主要取决于***硬件的计算能力。

步骤二、利用N个深度信息缺失的ORB特征点对，计算相机位姿变化的估计值ξ_P，利用M个深度信息完整的ORB特征点对，计算相机位姿变化的估计值ξ_Q，如采用直接线性变换法，再利用如下方程式，即可得到总估计值ξ₀，从而大大增加了用于计算的匹配成功的特征点对数量，提高了位姿变化的估计值的计算精度。

对于深度信息s缺失的ORB特征点对，如采用RANSAC算法计算估计值ξ_P，记初始帧的空间点齐次坐标为P＝(x,y,z,1)^T，设其在相邻帧上投影的归一化像素齐次坐标为p＝(u,v,1)^T，而此时相机变化的位姿即旋转矩阵和平移矩阵[R||t]未知，可将其定义为3*4增广矩阵T，包含了旋转与平移信息，则其转换关系如下：

其中，s表示ORB特征点的深度信息，用最后一行将其消去，可得到两个约束：

为了简化表示，进行如下定义：

T₁＝(t₁,t₂,t₃,t₄)^T,T₂＝(t₅,t₆,t₇,t₈)^T,T₃＝(t₉,t₁₀,t₁₁,t₁₂)^T

每个ORB特征点提供了两个线性约束，假设有N个ORB特征点对，则可以列出如下的线性方程组：

由于上式共有12维，因此只需要六组ORB特征点对即可实现其线性求解，得到增广矩阵T后，测试其它ORB特征点对，通过设定阈值选取模型内点并剔除误差点，从而提高增广矩阵的计算精度，当内点足够多时则完成ORB特征点对的优化并得到深度信息s缺失的估计值ξ_P，其中，ξ_P为李代数表达。

对于深度信息完整的ORB特征点对，参数模型的建立则方便许多，已知初始帧上的ORB特征点的空间点坐标为Q₁＝(X₁,Y₁,Z₁)，与之匹配的相邻帧上的ORB特征点的空间点坐标为Q₂＝(X₂,Y₂,Z₂)，想要找一个欧式变换R,t，使得：

Q₁＝RQ₂+t

类似于深度信息缺失的模型，R,t共有12维，而深度信息完整的ORB特征点对存在三个约束，故只需四对ORB特征点对即可完成线性求解，假设有M组点对，得到深度信息完整的ORB特征点对对应的估计值ξ_Q。

步骤三、构建融合了深度信息缺失的ORB特征点对和深度信息完整的ORB特征点对对应的误差信息的最小重投影误差模型，进而得到对应的雅可比矩阵J，主要是对于N个深度信息缺失的ORB特征点对，利用重投影方法，结合PnP算法，构建其对应的误差函数e_P；对于M个深度信息完整的ORB特征点对，利用重投影方法，结合ICP算法，构建其对应的误差函数e_Q；然后，构建融合误差函数e_P和误差函数e_Q的最小重投影误差模型e，进而得到对应的雅可比矩阵J。

具体如下：

步骤Ⅰ、对于N个深度信息缺失的ORB特征点对，记相邻两帧图像上匹配的ORB特征点对对应的空间点齐次坐标分别为P_i＝(x_i,y_i,z_i,1)^T，其相邻帧上的ORB特征点经过世界坐标系和像素坐标系的转换关系得到对应的像素齐次坐标为其转换关系如下：

其中，c_x、c_y分别表示相机的光心在像素坐标系中的坐标，f_x表示相机的焦距在像素坐标系的x轴分量，f_y表示相机的焦距在像素坐标系的y轴分量，表示相机的内参矩阵。

其中，s_i表示初始帧的ORB特征点在相邻帧上的投影对应的深度信息，K表示相机的内参矩阵，^表示反对称符号，

结合PnP算法，构建深度信息缺失的ORB特征点对的误差函数e_P。

其中，^表示反对称符号，

结合ICP算法，构建深度信息完整的ORB特征点对的误差函数e_Q；

步骤Ⅲ、构建最小重投影误差模型e，如下所示，该误差模型e融合了深度信息完整的ORB特征点对和深度信息缺失的ORB特征点对对应的误差信息，大大增加了匹配成功的特征点数量，提高了后续位姿优化的精度。

而最小重投影误差模型e的雅克比矩阵J的计算，需要根据深度信息完整的ORB特征点对和深度信息缺失的ORB特征点对对应的误差函数，分别计算对应的雅克比矩阵J_p和J_Q。

对于深度信息缺失的ORB特征点对，记点P_i′＝(x′_i,y′_i,z′_i)^T表示初始帧中ORB特征点P_i＝(x_i,y_i,z_i)^T乘以相机位姿变化的总估计值ξ₀后对应的点，P′＝(x′,y′,z′)^T＝(exp(ξ^)P)，对ξ^左乘扰动量δξ，然后使用链式法则，J_p列写如下：

其中，表示李代数上的左乘扰动，右边两项的其中第一项是关于投影点的导数，由相机投影模型可得：

而将两式相乘即可得到雅克比矩阵J_P，如下所示：

雅克比矩阵J_Q的推导与之相似，对于深度信息完整的ORB特征点对，点Q_i′＝(X′_i,Y_i′,Z′_i)^T表示初始帧中ORB特征点Q_i＝(X_i,Y_i,Z_i)^T乘以相机位姿变化的总估计值ξ₀后对应的点，使用李代数扰动模型即可得：

综上，可得由N个深度信息缺失的特征点对与M个深度信息完整的特征点对组成的最小重投影误差模型e的雅克比矩阵J，如下所示：

步骤四、根据总估计值ξ₀、最小重投影误差函数和雅可比矩阵J，利用非线性优化方法，计算优化后的总估计值ξ_k，从而完成相邻两帧图像拍摄过程中的相机位姿变化的估计。该非线性优化方法优选为高斯牛顿法，具体如下：

1)给定总估计值ξ₀；

2)对于第k次迭代，求出当前的雅克比矩阵J与误差e；

3)求解增量Δξ_k＝g/H，其中，H＝J^TJ,g＝-J^Te(ξ)；

4)若Δξ_k小于阈值，则停止，并输出优化后的总估计值ξ_k；否则，令ξ_k+1＝ξ_k+Δξ_k，返回第2)步。其阈值可根据实际情况而定。

为了验证本发明方法的可行性，我们将深度相机设置在Turtlebot移动机器人平台进行拍摄，设定直线运动速度0.3m/s，运动距离为3m，分别进行传统ICP算法、RANSAC+ICP算法和融合算法估计移动机器人的运动轨迹，并使用g2o求解器进行位姿优化求解，该g2o求解器是一个在SLAM领域广为使用的优化库，可以将非线性优化与图论相结合，大大提高***的实时性与精确性。其运动轨迹如图2所示，结果表明，采用本发明的方法的平均误差为1.98％，相比于传统ICP的8.64％与改进后的RANSAC+ICP算法的5.28％有着更小的误差。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。

Claims

1.一种用于深度相机的位姿估计方法，其特征在于包括以下步骤：

2.根据权利要求1所述的用于深度相机的位姿估计方法，其特征在于：对于N个深度信息缺失的ORB特征点对，利用重投影方法，结合PnP算法，构建其对应的误差函数e_P；对于M个深度信息完整的ORB特征点对，利用重投影方法，结合ICP算法，构建其对应的误差函数e_Q；然后构建融合误差函数e_P和误差函数e_Q的最小重投影误差模型e，进而得到对应的雅可比矩阵J。

3.根据权利要求2所述的用于深度相机的位姿估计方法，其特征在于所述雅可比矩阵J的计算方法包括以下步骤：

其中，si表示初始帧的ORB特征点在相邻帧上的投影对应的深度信息，K表示相机的内参矩阵，∧表示反对称符号，

结合PnP算法，构建深度信息缺失的ORB特征点对的误差函数e_P；

其中，∧表示反对称符号，

步骤Ⅲ、构建最小重投影误差模型e，如下所示；

其中，δξ表示扰动量，f_x表示相机的焦距在像素坐标系的x轴分量，f_y表示相机的焦距在像素坐标系的y轴分量，点P_i′＝(x′_i,y′_i,z′_i,1)^T表示初始帧中深度信息缺失的ORB特征点P_i＝(x_i,y_i,z_i,1)^T乘以相机位姿变化的总估计值ξ₀后对应的点，点Q_i′＝(X′_i,Y′_i,Z′_i)^T表示初始帧中深度信息完整的ORB特征点Q_i＝(X_i,Y_i,Z_i)^T乘以相机位姿变化的总估计值ξ₀后对应的点。

4.根据权利要求1所述的用于深度相机的位姿估计方法，其特征在于，利用高斯牛顿法，计算优化后的总估计值ξ_k的方法包括以下步骤：

1)给定总估计值ξ₀；

2)对于第k次迭代，求出当前的雅克比矩阵J与误差e；

3)求解增量Δξ_k＝g/H，其中，H＝J^TJ,g＝-J^Te(ξ)；

5.根据权利要求1所述的用于深度相机的位姿估计方法，其特征在于：对于N个深度信息缺失的ORB特征点对，利用RANSAC算法计算估计值ξ_P；对于M个深度信息完整的ORB特征点对，利用直接线性变换法计算估计值ξ_Q，再利用如下方程式，计算总估计值ξ₀。

6.根据权利要求1所述的用于深度相机的位姿估计方法，其特征在于：所述深度相机拍摄的每相邻两帧图像都有重叠区域，所述ORB特征点从重叠区域提取。