CN111913499B - 基于单目视觉slam及深度不确定性分析的云台控制方法 - Google Patents

Abstract

本发明涉及一种基于单目视觉SLAM及深度不确定性分析的云台控制方法，由三自由度云台配合单目相机实现，以ORB‑SLAM开源系统为主体框架。包括下列步骤：确定路标点在重建过程中，受单位二维像素扰动影响所引起的三维位置偏差；计算路标点的深度不确定性初值；当一个路标点被多个图像观测到时，需要进行不确定性的融合，即实时更新当前图像观测到的路标点的深度不确定性；建立并更新的路标点深度不确定性；计算使“跟踪已知路标”与“探索未知区域”二者达到平衡的最优云台朝向。

Description

基于单目视觉SLAM及深度不确定性分析的云台控制方法

技术领域

本发明涉及视觉SLAM(Simultaneous Localization And Mapping：同步定位与建图)领域，尤其涉及一种通过对环境地图的深度不确定性进行优化分析，从而实现的基于云台主动控制的方法。

背景技术

机器人在空间中移动时对环境的感知与自身位姿的估计是机器人研究领域的关键问题。针对该问题的一项重要研究为移动机器人的同时定位与地图构建，它是指搭载特定传感器的主体，在没有先验环境信息的情况下，在运动过程中建立环境地图，同时估计自身的运动状态。基于相机传感器的视觉SLAM是近二十年逐渐兴起的一类解决方案，它仅通过相机随机器人运动时拍摄的连续图像，就可以实现对自身运动状态的推断与场景的感知，在传感器方面具有成本低、体积小、信息量丰富的优势。

2014年，Engel等人(Engel J,Schops T,Cremers D.LSD-SLAM:Large-scaledirect monocular SLAM[C].IEEE European Conference on Computer Vision,2014:834-849)提出一种基于光流的单目视觉SLAM算法。该算法通过对大量成对的像素级别立体构成的小基线进行滤波，从而显式地检测尺度漂移，实时地重建关键帧的位姿图及相关的半稠密深度图。但该算法只能在相机有较小的位移变化时起作用，同时算法依赖的“灰度不变假设”是很强的约束，在实际情况中极难满足。另外，算法由于缺少回环检测功能，无法判断机器人是否运动到之前经历过的位置，因此无法消除位姿估计的累积误差，在系统完善度方面有着较大劣势。

2015年，Mur-Artal等人(Mur-Artal R,Montiel J M,Tardos J D.ORB-SLAM:AVersatile and Accurate Monocular SLAM System[J].IEEE Transactions onRobotics,2015,31(5):1147-1163)提出一种基于特征点的单目视觉SLAM算法。该算法采用ORB(Oriented FAST and Rotated BRIEF)特征以提高系统的特征点利用率，建立特征追踪、局部位姿优化和回环检测三个独立的线程，是目前功能最完善、应用最广泛的单目视觉SLAM系统之一。但这种算法对于相机传感器的利用类似于惯性测量单元(即相机只能“被动”保持与机器人运动同步)，而这种利用模式使得算法在特征追踪环节，容易出现因干扰抖动而导致的跟踪失效问题。

受到生物视觉注意机制的启发，当人类进入陌生环境对周边事物进行探查时，脖颈及眼球会协同转动以扩大观察范围。因此，针对上述特征点跟踪失效问题的一种合理解决方案，是利用多自由度云台搭载相机自由改变朝向，进而实现对特征点的“主动”跟踪。

2002年，Davison等人(Davison A J,Murray D W.Simultaneous localizationand map-building using active vision[J].IEEE Transactions on Pattern Analysisand Machine Intelligence,2002,24(7):865-880)最先提出一种基于四自由度云台与双目相机的主动视觉SLAM方法，通过对相机观测模型的协方差矩阵进行分析，预测场景中最具不确定性的人工特征位置，并以此作为相机的观测方向。但该算法对于观测模型协方差矩阵的分析依赖于扩展卡尔曼滤波的迭代更新，不适用于具有大量路标且需要实时更新状态量的应用场景。

2006年，Vidal-Calleja等人(Vidal-Calleja T,Davison A J,Andrade-Cetto J,et al.Active control for single camera SLAM[C].IEEE International Conferenceon Robotics and Automation,2006:1930-1936)提出一种基于信息论的主动视觉SLAM方法。该算法通过最大化互信息增益指导机器人的位置移动，使机器人尽可能探索“未知”区域；通过最大化Fisher信息矩阵的迹改变相机朝向，使相机尽可能跟踪“已知”路标。此方案虽然有助于平衡未知信息的探索与已知信息的利用，但破坏了SLAM问题的独立性，即主动地改变机器人位置可能会影响到诸如避障、目标物体追踪等其他任务。

2008年，Frintrop等人(Frintrop S,Jensfelt P.Attentional landmarks andactive gaze control for visual SLAM[J].IEEE Transactions on Robotics,2008,24(5):1054-1065)提出一种可以促进回环检测的主动视觉SLAM系统。该算法采用串行机制(即探索未知环境与利用已知信息具有不同的优先级)，并追踪具有较高不确定性的路标。但该算法对于路标的不确定性没有明确的定量分析，只是定性地指出路标越贴近于机器人的行进方向，其位置不确定性越大。

2016年，Manderson等人(Manderson T,Shkurti F,Dudek G.Texture-aware SLAMusing stereo imagery and inertial information[C].IEEE InternationalConference on Computer and Robot Vision,2016:456-463)提出一种融合惯导信息的双目主动视觉SLAM算法。该算法利用预训练的LBP(Local Binary Patterns)特征描述分类器，对图像中各区块进行纹理特性评估，进而实现相机朝向的控制。但该算法控制相机始终凝视图像中具有较高纹理特性的区块，因此忽略了对于未知区域的探索，不利于环境地图构建。

综上所述，引入对相机位姿的主动控制量，能够有效解决基于特征点的视觉SLAM系统存在的跟踪失效问题。但现有的主动控制策略缺少定量分析，或者以牺牲原系统的完整性、独立性为代价。

发明内容

本发明针对基于特征点的单目视觉SLAM算法存在的特征跟踪失效问题，提出一种基于深度不确定性分析的相机主动控制策略，在保证原有SLAM系统独立性与完整性的前提下，实现跟踪已知路标与探索未知区域之间的平衡；当系统处于已长期运行的状态时，易于触发回环检测功能，进而减少机器人位姿的累积误差。技术方案如下：

一种基于单目视觉SLAM及深度不确定性分析的云台控制方法，由三自由度云台配合单目相机实现，以ORB-SLAM开源系统为主体框架，包括下列步骤：

1)确定路标点在重建过程中，受单位二维像素扰动影响所引起的三维位置偏差：

其中，Dist_base表示相邻两帧间的基线长度，Dist_est表示初步估计的路标深度值，角度α、β分别表示路标点与相邻两帧构成的位移向量与基线的夹角，Δβ表示单位二维像素扰动引起的角度偏差；

2)计算路标点的深度不确定性初值；

3)当一个路标点被多个图像观测到时，需要进行不确定性的融合，即实时更新当前图像观测到的路标点的深度不确定性：

其中，δ₁和δ₂分别代表两次对路标点的不确定性计算，δ_fuse代表更新结果；

4)建立并更新的路标点深度不确定性；

5)计算使“跟踪已知路标”与“探索未知区域”二者达到平衡的最优云台朝向:

构建相机的有效观测区域判别函数h(T，P)，用以判断相机在某一姿态T是否可以观测到路标点P：

其中，ε为单位阶跃函数，当函数内部大于0时，输出为1；否则输出为0；向量为路标点相对于相机中心的位移向量，/>为沿相机坐标系朝向的单位向量；/>代表向量/>与/>之间的夹角，由余弦定理获得；当路标点位于相机有效观测区域内时，判别函数h(T，P)结果为1；反之为0；

结合路标点的深度不确定性，构建如下优化问题：

式(4)为目标函数，式(5)为约束条件，优化变量为相机姿态矩阵T，求解使目标函数取最小值、且满足约束条件的相机姿态矩阵T，即可获得下一时刻的云台控制量；其中，N为环境地图中路标点总数，P_i为第i个路标点，T_k为相机当前时刻位姿，为路标点P_i在当前时刻的深度不确定性；ρ为平衡参数，用以调节“跟踪已知路标”与“探索未知区域”在控制决策中的影响力；a为度量参数；R为3×3的旋转矩阵，t为3×1的平移向量，t_k为当前时刻相机的平移向量，/>为机器人行进速度，Δt为云台控制周期。

本发明主要优点及特色体现在如下两个方面：

1、本发明在保证原有单目视觉SLAM系统独立性与完整性的前提下，配合云台转动实现了跟踪已知路标与探索未知区域之间的平衡。

2、本发明使系统在长期运行一段时间后，更加主动地触发回环检测功能，进而有效地降低机器人位姿估计的累积误差。

附图说明

图1为单目视觉SLAM系统的整体框架图

图2为路标点深度不确定性计算原理图

图3为不确定性更新示例图

图4为单目相机有效观测区域俯视图

图5为位置约束下的云台朝向规划问题示意图

具体实施方式

本发明的基于深度不确定性分析的单目主动视觉SLAM方法，需要三自由度云台配合单目相机实现。另外算法以ORB-SLAM开源系统(公开于Github网站，网址：https:// github.com/raulmur/ORB_SLAM2)为主体框架，并在原有功能的基础上进行完善。

[1]计算路标点的深度不确定性初值

路标点在重建过程中，受单位二维像素扰动影响所引起的三维位置偏差：

其中，Dist_base表示相邻两帧间的基线长度，Dist_est表示初步估计的路标深度值，角度α、β分别表示路标点与相邻两帧构成的位移向量与基线的夹角，Δβ表示单位二维像素扰动引起的角度偏差，考虑到相机焦距f远大于一个像素的距离，Δβ可以近似为arctan(1/f)。

[2]本发明提出，当一个路标点被多个图像观测到时，需要进行不确定性的融合，即实时更新当前图像观测到的路标点的深度不确定性：

其中，δ₁和δ₂分别代表两次对路标点的不确定性计算，δ_fuse代表更新结果。

[3]本发明提出，利用前文不断建立并更新的路标点深度不确定性，计算使“跟踪已知路标”与“探索未知区域”二者达到平衡的最优云台朝向。首先，构建相机的有效观测区域判别函数h(T，P)，用判断相机在某一姿态T是否可以观测到路标点P：

其中，ε为单位阶跃函数，当函数内部大于0时，输出为1；否则输出为0。向量为路标点相对于相机中心的位移向量，/>为沿相机坐标系朝向的单位向量。/>代表向量与/>之间的夹角，可由余弦定理获得。上式主要由两部分构成，分别用于检测路标点与相机中心的距离，及其偏离相机中轴线的角度。当路标点位于相机有效观测区域内时，上式结果为1；反之为0。

随后，结合路标点的深度不确定性，构建如下优化问题：

式(4)为目标函数，式(5)为约束条件，优化变量为相机姿态矩阵T，求解使目标函数取最小值、且满足约束条件的相机姿态矩阵T，即可获得下一时刻的云台控制量。其中，N为环境地图中路标点总数，P_i为第i个路标点，T_k为相机当前时刻位姿，为路标点P_i在当前时刻的深度不确定性。ρ为平衡参数，用以调节“跟踪已知路标”与“探索未知区域”在控制决策中的影响力。a为度量参数，使得待相加的两项处于同一度量标准，本发明令其为每帧额定提取的特征点数量的倒数，即a＝1/1000。R为3×3的旋转矩阵，t为3×1的平移向量，t_k为当前时刻相机的平移向量，/>为机器人行进速度，可通过原系统路标跟踪线程中相邻帧间的位置差分获得，Δt为云台控制周期，本发明中设定为100毫秒。

下面结合附图和实施例具体说明：

[1]本发明在保留原系统功能的基础上，在环境地图初始化环节增加了对路标点深度不确定性的初值计算。所谓“深度不确定性”，即路标点在重建过程中，受单位像素扰动影响所引起的位置偏差。以图2为例，单目相机在O₁和O₂两个不同位置，经特征匹配及三角化重建得到三维路标点P，设p₁和p₂分别为点P在两成像平面的投影点。假设在特征匹配阶段沿极线出现一个像素坐标的误差扰动(即左侧图像的特征点p₁与右侧图像的p′₂配对)，则由对极约束可知，在扰动下重建的路标点应出现在O₁P延长线上的一点P′。在已知O₁、O₂、P三点坐标的前提下，O₁P′的距离值可由正弦定理得到：

其中，角度α和β可以由余弦定理得到。考虑到相机焦距f远大于一个像素的距离，因此单位误差扰动引起的角度偏差Δβ可以近似为arctan(1/f)。基于以上分析，当特征匹配沿极线出现单位像素扰动时，重建得到的点P′与原始路标点P的位置偏差δ＝||O₁P′||-||O₁P||，因此路标的深度不确定性可以用偏差δ表征。

[2]本发明在插入新关键帧环节更新与该帧具有关联的路标点的深度不确定性，即当一个路标点被多个关键帧观测到时，需要进行不确定性的融合。以图3为例，三维路标点P是利用x₁和x₂两个不同关键帧进行地图重建得到的，设其初始深度不确定性为δ₁。当出现新的观测到该点的关键帧x₃时，通过其与关键帧x₁之间的几何约束，接[1]中所述可求得一个暂态更新量δ₂，并采用如下公式进行不确定性的融合：

由上式分析可知，因为深度不确定性δ₁、δ₂恒为正数，所以δ_fuse恒小于δ₁、δ₂，即当观测到该点的关键帧越多，其深度不确定性也随之减小。上式可以利用信息融合理论解释，假设两个深度值的概率分布分别是以δ₁和δ₂为方差的高斯分布，则融合后的概率依旧服从高斯分布，且方差为δ_fuse。对后续出现的观测到该路标点的关键帧，则在δ_fuse的基础上进行迭代更新。

[3]本发明的核心创新在于云台主动控制线程。首先，构建相机的有效观测模型。以图4为例，相机有效观测区域的俯视图可以近似看作以r为半径，θ为圆心角的扇形(r与θ可通过预标定得到)。则相机在位姿T时是否可以观测到路标点P，可以进一步表述为：

其中，ε为单位阶跃函数，当函数内部大于0时，输出为1；否则输出为0。向量为路标点P相对于相机中心的位移，/>为相机坐标系沿x轴方向的单位向量。/>代表向量/>与/>之间的夹角，可由余弦定理获得。上式主要由两部分构成，分别用于检测路标点与相机中心的距离，及其偏离相机中轴线的角度。当路标点位于相机有效观测区域内时，上式结果为1；反之为0。

随后，结合前文中不断建立并更新的路标点深度不确定性，构建如下待优化目标函数：

其中，N为环境地图中路标点总数，P_i为第i个路标点，T_k为相机当前时刻位姿，为路标点P_i在当前时刻的深度不确定性。ρ为平衡参数，用以调节“跟踪已知路标”与“探索未知区域”在控制决策中的影响力。a为度量参数，使得待相加的两项处于同一度量标准，本发明令其为每帧额定提取的ORB特征点数量的倒数，即a＝1/1000。考虑到机器人潜在的避障、目标物体追踪等其他任务，相机只可以设定朝向，但不可以设定位置，因此添加约束条件：

其中，R为3×3的旋转矩阵，t为3×1的平移向量，t_k为当前时刻相机的平移向量，为机器人行进速度，可通过原系统路标跟踪线程中相邻帧间的位置差分获得，Δt为云台控制周期，本发明中设定为100毫秒。上述约束条件使得优化变量T中的平移向量t固定，即预先估计机器人下一时刻的位置，并以此作为相机的位置，后续只针对旋转矩阵进行优化，如图5所示。通过求解使上述有约束优化问题的最优解E_k，分解并保留其旋转矩阵R，即可得到期望的云台朝向设定值。

Claims (2)

1.一种基于单目视觉SLAM及深度不确定性分析的云台控制方法，由三自由度云台配合单目相机实现，以ORB-SLAM开源系统为主体框架，包括下列步骤：

1)确定路标点在重建过程中，受单位二维像素扰动影响所引起的三维位置偏差：

其中，Dist_base表示相邻两帧间的基线长度，Dist_est表示初步估计的路标深度值，角度α、β分别表示路标点与相邻两帧构成的位移向量与基线的夹角，Δβ表示单位二维像素扰动引起的角度偏差；

2)计算路标点的深度不确定性初值；

3)当一个路标点被多个图像观测到时，需要进行不确定性的融合，即实时更新当前图像观测到的路标点的深度不确定性：

其中，δ₁和δ₂分别代表两次对路标点的不确定性计算，δ_fuse代表更新结果；

4)建立并更新的路标点深度不确定性；

5)计算使“跟踪已知路标”与“探索未知区域”二者达到平衡的最优云台朝向:

构建相机的有效观测区域判别函数h(T，P)，用以判断相机在某一姿态T是否可以观测到路标点P：

其中，ε为单位阶跃函数，当函数内部大于0时，输出为1；否则输出为0；向量为路标点相对于相机中心的位移向量，/>为沿相机坐标系朝向的单位向量；/>代表向量/>与/>之间的夹角，由余弦定理获得；当路标点位于相机有效观测区域内时，判别函数h(T，P)结果为1；反之为0；

结合路标点的深度不确定性，构建如下优化问题：

式(4)为目标函数，式(5)为约束条件，优化变量为相机姿态矩阵T，求解使目标函数取最小值、且满足约束条件的相机姿态矩阵T，即可获得下一时刻的云台控制量；其中，N为环境地图中路标点总数，P_i为第i个路标点，T_k为相机当前时刻位姿，为路标点P_i在当前时刻的深度不确定性；ρ为平衡参数，用以调节“跟踪已知路标”与“探索未知区域”在控制决策中的影响力；a为度量参数；R为3×3的旋转矩阵，t为3×1的平移向量，t_k为当前时刻相机的平移向量，/>为机器人行进速度，Δt为云台控制周期。

2.根据权利要求1所述的方法，其特征在于，a＝1/1000。