一种基于智能机器人地面特征的PTAM改进方法

摘要

一种基于智能机器人地面特征的PTAM改进方法，首先，完成参数校正，这包括参数定义与相机校正；然后利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，运用FAST角点检测算法提取当前图像中的特征信息，建立角点特征间的数据关联，得到位姿估计模型；在地图的初始绘制阶段，获取两个关键帧将相机架设在移动机器人上；在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联；实现三维稀疏地图的初始化后，更新关键帧并利用极线搜索与块匹配方法建立特征点亚像素精度映射关系，结合位姿估计模型实现相机精确重定位。最后将匹配点投影到空间，完成当前全局环境三维地图创建。

说明书

技术领域

本发明属于机器人视觉领域，涉及一种基于地面特征的PTAM算法改进。

背景技术

随着机器人与人类的关系越来越紧密，智能机器人的相关技术受到了极大的重视。移动机器人的同步定位与建图(Simultaneous Localization and Mapping,SLAM)是当前智能移动机器人最主流的定位技术之一。它实际上就是一个运动估计问题，利用传感器获取的内部和外部数据，计算出移动机器人在某一时刻的位置，同时建立所依赖的地图模型。基于视觉的SLAM是属于视觉测量的研究范畴，由于视觉传感器拥有自身独特的优点：体积小，重量轻，价格便宜，安装方便，提取的外部信息非常丰富。这些优势进一步促进了当前基于视觉的SLAM的研究和应用。申请号为201310678018.2提供了一种采用SURF作为单目视觉SLAM的特征检测算子，分别从感兴趣点的检测、SURF描述子的生成及SURF点匹配等三个方面进行了创新和改进，提出了基于区域增长的SURF特征匹配方法完成机器人的同时定位与地图创建，使得在SURF描述子匹配过程中，某一描述子仅与最可能与之匹配的描述子进行比对，从而显著的减少了单目视觉SLAM问题中的比对次数，提高了匹配速度。但该方法依然存在不能建立米制地图以及无法提供三维特征信息的问题，算法鲁棒性与实时性有待进一步提高。

从运动中创建环境模型技术是一种同时求解摄像机运动轨迹和场景结构模型的方法。作为SFM的代表性方法，G.Klein等人于2007年在增强现实(AR)领域首先提出了PTAM(Parallel Tracking and Mapping)的概念，用来实时地解决环境识别问题。但未改进的PTAM算法也存在无法建立米制地图的问题，同时对相机的移动有着严格限制。

发明内容

针对以上存在的问题，本发明提出了一种基于地面特征的PTAM改进算法，在加入地面特征的基础上，PTAM对单目摄像机的当前位姿与三维空间点进行同步估计，在利用FAST角点方法更新检测特征点的同时，用最优的局部与全局光束平差法，不断地更新摄像机与三维特征点的位姿，完成了高精度定位与地图创建，该发明有效解决了无法创建米制地图的问题，同时在实时性及三维特征提取上有着极大的提高，对于解决机器人导航定位、三维重建中的相关问题有着重要意义。

本发明采用如下的技术方案：

首先，完成参数校正，这包括参数定义与相机校正；接着对关键帧中的特征进行跟踪，再实现位姿的初始化，再此基础上对平面参数进行估计；然后利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，运用FAST角点检测算法提取当前图像中的特征信息，建立角点特征间的数据关联，得到位姿估计模型；在地图的初始绘制阶段，获取两个关键帧将相机架设在移动机器人上，通过相机与地面标定后开始进行初始化过程；在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联，由位姿估计模型获取当前位姿初始估计，再运用基于地面特征的位姿估计方法获取当前位姿的准确估计；实现三维稀疏地图的初始化后，更新关键帧并利用极线搜索与块匹配方法建立特征点亚像素精度映射关系，结合位姿估计模型实现相机精确重定位。最后将匹配点投影到空间，完成当前全局环境三维地图创建。同时利用全局与局部集束调整完成算法的一致性优化。

具体包括以下步骤：

步骤1，参数校正 

步骤1.1，参数定义 

(1)机器人位姿 

在移动机器人起始处建立世界坐标系，在机器人移动过程中，其位姿可表示为：

$\left(\begin{array}{c}{T}_r=\left(\begin{array}{cc}{R}_r& t_r\\ 0& 1\end{array}\right)\\ R_r=\left(\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right),t_r=\left(\begin{array}{c}x\\ y\\ 0\end{array}\right)\end{array}\right)---\left(1\right)$

由上式可知移动机器人位姿可由x＝(x，y,θ)描述，其中，x，y分别表示移动机器人在x轴，y轴方向的平移量，θ为移动机器人绕z轴的旋转角度。

在世界坐标系下，相机的位姿可表示为:

$T_c=\left(\begin{array}{cc}{R}_c& t_c\\ 0& 1\end{array}\right)---\left(2\right)$

其中，T_c∈SE(3)，SE(3)为特殊刚体变换群，R_c为3×3旋转矩阵，t_c为3×1平移矩阵。该位姿矩阵建立了世界坐标系与相机坐标系下点p_c，p_w的对应关系，即p_c＝T_cp_w。在附图2所示，本研究中相机固定在移动机器人上，相机与机器人间存在固定的旋转平移关系T_cr。在机器人移动过程中，若某一时间间隔机器人位姿增量为T_rs，与之对应，相机的位姿增量为T_cs，则：

T_rs＝T_rcT_csT_cr    (3) 

其中，$T_{\mathrm{cr}}=T_{\mathrm{rc}}^{-1}.$

(2)地平面标定参数

将相机固定在移动机器人上，并将标定靶标平放在地面。相机通过获取具有标定靶标的图像信息建立相机与机器人间位姿关系。T_cp∈SE(3)为相机与标定靶标间的变换矩阵，R_cp，t_cp分别表示T_cp的旋转与平移分量，其逆变换T_cp＝T_p^-_c¹。假定机器人的坐标原点与相机保持一致，且机器人的x轴与相机的z轴朝向相同。在靶标坐标系下，机器人的x方向向量可由T_cp的第3行向量的前两个分量表示。此时，机器人坐标系与靶标坐标系的变换关系可表示为：

p_r＝T_rpp_p

$T_{\mathrm{rp}}=\left(\begin{array}{cccc}\cos \gamma & \sin \gamma & 0& x_r\\ -\sin \gamma & \cos \gamma & 0& y_r\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

其中，γ为机器人坐标系与靶标坐标系的旋转角度，(x_r,y_r)为机器人原点在靶标坐标系下的坐标，该坐标可由相机与靶标的平移向量确定。因此机器人与相机间的变换关系可表示为T_cr＝T_cpT_pr，根据相机与靶标平面间的位姿关系，可获取在相机坐标系下的地平面方程：

(0,0,1,0)T_pcp_c＝0    (5) 

其中，p_c为相机坐标系下三维点，地平面参数可表示为(n^T,d)，且n＝(0,0,1)R_pc，d＝t_p(3)。

步骤1.2，相机校正 

由于透镜在制造上的原因会存在畸变，畸变主要分为由透镜形状引起的镜向畸变和由摄像机组装过程引起的切向畸变。本文采用FOV模型实现对单目相机的矫正，校正模型如图3。该模型是由Devernay和Faugeras提出的一种针对广角相机去畸变方法，其数学描述如下：

$u_d=\left(\begin{array}{c}{u}_0\\ v_0\end{array}\right)+\left(\begin{array}{cc}{f}_u& 0\\ 0& f_v\end{array}\right)\frac{r_d}{r_u}{x}_u$

$r_d=\frac{1}{\omega }\arctan \left(2r_u\tan \frac{\omega }{2}\right)---\left(6\right)$

$r_u=\frac{\tan \left(r_d\omega \right)}{2\tan \frac{\omega }{2}}$

式中，x_u是像素归一化坐标，u_d是畸变后像素坐标，ω为畸变系数，r_d,r_u分别为矫正前后归一化坐标到坐标系中心的距离。

利用上述相机模型，将图像像素坐标映射到归一化坐标平面，同时结合相机内参数矩阵K，实现图像畸变矫正，即：

u＝Kx_u    (7)

步骤2，基于地面特征的初始化

步骤2.1，特征跟踪 

FAST角点检测是由Edward Rosten和Tom Drummond提出的一种简单快速的角点检测算法。该算法利用像素点某个邻域(通常为圆形区域)内相邻像素点灰度值的差异，判断该像素点是否为角点，如下式：

$N=\underset{S\in C\left(u_f\right)}{\Sigma }|I\left(s\right)-I\left(u_f\right)|---\left(8\right)$

其中，u_f为候选角点，s为候选角点邻域内任意一点，c(u_f)表示u_f的邻域，I(x)为在x处图像灰度值，若N超出阈值条件，该候选点即为角点。

在相机跟踪线程前，系统利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，采用FAST的角点检测算法，结合块搜索的方式建立角点特征间的数据关联，利用投影关系预测三维点与当前帧中特征点的对应关系，并在位置附近固定区域进行搜索，以获取准确的匹配关系。

步骤2.2，位姿初始化 

在机器人移动过程中，通过相机获取地面信息，并利用FAST算法实现角点特征的跟踪。根据上述数据关联信息，可建立平面点的单应性关系，即：

π(Hu_r)＝u_p    (9)

其中，π(p)为投影函数，p＝(x,y,z),π(p)＝(x/z,y/z,1)，u_r，u_p分别为参考帧与当前帧下对应特征点。根据相机平移量T_c以及地平面方程参数(n^T,d)，可建立平面点的单应性关系：

H＝KT_c(I|n_d)^TK^-1    (10) 

其中，n_d＝n/d，K为相机内参数矩阵。将式(3)代入式(10)，此时机器人的位姿与地面特征单应性关系可描述为：

H＝KT_rcT_rsT_rc(I|n_d)^TK^-1    (11) 

利用上述图像特征点的单应性关系，可建立位姿估计模型：

$\left(\begin{array}{c}x=\arg \underset{x}{\min }f\left(x\right)\\ f\left(x\right)=\underset{p}{\Sigma }{\left|\right|r\left(x\right)\left|\right|}^2\end{array}\right)---\left(12\right)$

其中，r(x)＝π(H(x)u_ref)-u_p为单应性关系误差函数，即投影误差函数，H(x)为单应性矩阵。根据小位移假设，误差函数r的一阶泰勒展开表示为：

$r\left(\xi \right)\left(0\right)+\frac{\partial r}{\partial \xi }{|}_{\xi =0}\xi ---\left(13\right)$

此时，能量函数的极值条件为：

$\frac{\partial f}{\partial \xi }=\Sigma J^{T}r=0---\left(14\right)$

即：

$\left(\begin{array}{c}\underset{p}{\Sigma }{J}^T(r\left(0\right)+\mathrm{J\xi })=0\\ \xi =-{\left(\underset{p}{\Sigma }{J}^{T}J\right)}^{-1}\underset{p}{\Sigma }{J}^{T}r\left(0\right)\end{array}\right)---\left(15\right)$

其中，可通过迭代求解上式获取移动机器人的位姿估计。为了进一步提高定位精度，本文引入鲁棒性权值构建基于M-estimation的位姿估计模型，此时能量函数表示为：

$f\left(x\right)=\underset{p}{\Sigma }w\left(r\right){\left|\right|r\left|\right|}^2---\left(16\right)$

其中，w(x)为鲁棒性Tukey加权函数，此时，能量函数的极值条件为：

$\xi =-{\left(\underset{p}{\Sigma }{J}^{T}w\left(r\right)J\right)}^{-1}\underset{p}{\Sigma }{J}^{T}w\left(r\right)r\left(0\right)---\left(17\right)$

步骤2.3，平面参数估计

机器人利用基于地面特征的单应性关系实现机器人定位，但由于非地面特征的影响，难以保证定位精度，因此本发明采用一种快速的非地面特征去除方法。根据平行向量叉乘方式构建法向估计模型。实验发现，由于角点检测存在一定误差，使得理想叉乘条件无法满足，法向估计模型适应度差。利用投影误差r作为判定依据，从而确定非地面特征信息。但随着非地面特征与观察点间距离减小，该方法区分度明显降低。相比上述方法，本发明在位姿初始估计的基础上，利用单应性矩阵直接推导法向估计模型，通过获取其与地平面方程参数的差异，判别该角点是否为地面特征信息。根据平面单应性关系式，可建立平面参数n_d的估计模型：

$f\left(n_d\right)=\underset{p}{\Sigma }{\left|\right|r\left(n_d\right)\left|\right|}^2---\left(18\right)$

其中，r(n_d)＝π(H(n_d)u_ref)-u_p为相应投影误差函数。与位姿估计求解过程类似，在小位移假设的基础上，可根据误差函数的一阶泰勒公式建立式(18)的求解模型。

此时，平面参数可通过迭代式(18)获取：

$n_d=n_d+\hat{n}$

$\hat{n}=-{\left(\underset{p}{\Sigma }{J}_n^T{J}_n\right)}^{-1}\underset{p}{\Sigma }{J}_n^{T}r\left(n_d\right)---\left(19\right)$

其中，$J_n=\frac{\partial r\left(n_d\right)}{\partial n_d}=\frac{\partial \pi }{\partial p}\frac{\partial H}{\partial n_d}.$

步骤3，位姿估计 

位姿估计模块的主要功能是完成相机位姿的实时更新。其具体实施过程如下。在完成初始化工作后，根据投影误差，建立位姿估计模型，其数学描述如下：

$\xi =\underset{\xi }{\arg \min }\mathrm{\Sigma Obj}(\frac{\left|e_j\right|}{{\sigma }_j},{\sigma }_T)---\left(20\right)$

其中，e_j是投影误差，且为Tukey双权目标函数，σ_T为特征点匹配标准差的无偏估计值，μ为相机位姿六元组描述向量。

对于跟踪中存在的失败情况，提出特征观察比值用以评估每帧图像的跟踪质量。若比值低于设定阈值，可以为跟踪效果较差，则继续跟踪，但是系统不会将该帧更新到地图中；若在多帧图像的跟踪中都出现了特征观察比值低于设定阈值的情况时，即可认为属于“跟丢”的情况，那么就初始化跟踪过程。通过以上步骤实现了位姿的精确估计，跟踪过程得以正常进行。

步骤4，地图创建

步骤4.1，地图初始化

系统利用利用地面特征与标准立体相机算法模型建立当前环境初始化地图。在地图的初始化过程中，利用图像中FAST角点匹配关系，结合RANSAC算法，采用基于地面特征的初始位姿估计方法，运用三角测量法计算当前特征点处的三维坐标，并确定起始点为全局世界坐标系原点，完成三维稀疏地图的初始化。

在地图的初始绘制阶段，获取两个关键帧的具体过程如下，将相机架设在移动机器人上，通过相机与地面标定后开始进行初始化过程。在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联，通过求解式(12)获取当前位姿初始估计。当该初始估计超过设定阈值d₀时，运用2.2中基于地面特征的位姿估计方法获取当前位姿的准确估计。

步骤4.2，关键帧更新与极线搜索

地图初始化时，仅仅包含两个关键帧，并且只能对邻近较小的体积空间进行描述，在相机离开初始位置时，需要添加新的关键帧以及地图特征。若相机与当前关键帧间图像帧数超出阈值条件，相机跟踪效果为最佳，且相机与最近的地图关键点距离尽量小时，将自动执行添加关键帧过程。由于实时性的约束，跟踪系统可能只是测度到了帧中可视特征的子集，因此映射线程需要重新计划和测量余下的地图特征。

在关键帧的更新中，系统首先将会对新增加的关键帧中所有角点进行Shi-Tomas评估，获取当前具有显著特征的角点信息，成功观测的特征点附近的显著点将会被丢弃，保留下来的显著性点将作为候选的地图节点。由于新的地图节点需要已知深度信息，仅靠单帧图像无法获取，因此需要选取与之最近的关键帧作为第二视图，利用极线搜索与块匹配方法建立特征点亚像素精度映射关系。在第二视图中，采用零均值SSD法，结合位姿估计模型实现相机精确重定位。最后将匹配点投影到空间，以生成当前全局环境三维地图。

步骤4.3，优化

利用最优的局部与全局LM(Levenberg-Marquardt)集束调整算法实现当前地图全局一致性优化以提高鲁棒性。该集束调整算法的数学描述为：

$\left\{\right\{{\mu }_2...{\mu }_N\},\{{p^{\text{'}}}_1...{p^{\text{'}}}_M\left\}\right\}=\underset{\left\{\right\{\mu \},\{p\left\}\right\}}{\arg \min }\underset{i\in X\in Y}{\Sigma }\underset{j\in S_i}{\Sigma }\mathrm{Obj}(\frac{\left|e_{\mathrm{ji}}\right|}{{\sigma }_{\mathrm{ji}}},{\sigma }_T)---\left(21\right)$

其中，σ_ji为在第i个关键帧中，FAST特征点匹配标准差的无偏估计，μ_i表示第i个关键帧的位姿六元组描述向量，p_i为全局地图中的点。

通过不断的集束调整，使得(21)中的稳定代价函数极小。全局集束调整所有关键帧的位姿以及地图点的位置，同时利用结构-运动问题中的内在稀疏性使得整体矩阵分解的复杂度呈现立方级别的下降，由O((N+M)³)降至O(N³)，但在多数的估算情况中又受到生成的外点矢量积的制约，随着地图的生长，这个运算量会显著增加。

针对以上问题，引入局部集束调整，这里只对关键帧的子集位姿进行调整，此时的目标函数变为：

$\left\{\right\{{\mu }_{\chi }\in X\},\{{p^{\text{'}}}_z\in Z\left\}\right\}=\underset{\left\{\right\{\mu \},\{p\left\}\right\}}{\arg \min }\underset{i\in X\in Y}{\Sigma }\underset{j\in Z\cap S_i}{\Sigma }\mathrm{Obj}(i,j)---\left(22\right)$

X为系列关键帧的待调整集合，Y为固定帧集，Z为地图点子集。集合X由地图中一个最新的关键帧以及4个最接近的关键帧构成，而关键帧的所有地图中的可视点又组成了集合Z，最后Y包含了所有由Z中测量数据所获取的关键帧。由此，局部集束调整优化了最新、最近的关键帧的位姿，更新了地图中的所有点，同时复杂度也显著降低，不超过O(NM)。

与原PTAM算法相比，本发明加入了地面特征来对其进行改进，实现了如下的几个优点：

(1)改进了初始化过程，有效解除了PTAM算法对相机移动的严格限制。

(2)可有效建立相对准确的米制地图，为进一步进行环境测量提供可靠保证。

附图说明

图1为本发明所涉及方法的流程图；

图2相机校正模型；

图3平面参数标定示意图。

具体实施方式

下面结合附图对本发明专利进行进一步详细说明。

基于地面特征的PTAM改进算法流程图如附图1所示，具体包括以下几个步骤：

步骤1，参数校正 

步骤1.1，参数定义 

由机器人坐标系与世界坐标系的关系，构建机器人位姿表示，而由相机与靶标平面间的位姿关系确定地平面标定参数。

步骤1.2，相机校正 

采用FOV模型实现单目相机的校正，将图像像素坐标映射到归一化坐标平面，同时结合相机内参数矩阵K，实现图像畸变矫正。

步骤2，基于地面特征的初始化

步骤2.1，特征跟踪 

获取环境纹理信息，提取特征，建立角点特征关联。

步骤2.2，位姿初始化 

建立单应性关系以及位姿初始化估计模型，实现位姿初始化。

步骤2.3，平面参数估计

步骤3，位姿估计 

在完成初始化的基础上，根据基于地面特征的姿估计模型，实现相机的初始化位姿估计。更进一步，PTAM根据相机初始化位姿，采用极线搜索方式，建立图像金字塔中角点特征亚像素精度匹配关系，并重新带入位姿估计模型，实现相机精确重定位。

步骤4，地图创建

步骤4.1，地图初始化

系统利用标准立体相机算法模型建立当前环境初始化地图

步骤4.2，关键帧更新与极线搜索

添加新的关键帧并利用极线搜索与块匹配方法建立特征点亚像素精度映射关系，结合位姿估计模型实现相机精确重定位。最后将匹配点投影到空间，完成当前全局环境三维地图创建。

步骤4.3，优化

利用全局与局部集束调整实现地图的一致性优化。