基于Dog-Leg算法的服务机器人视觉SLAM系统

2021-04-22王素芳张维存

计算机工程与设计 2021年4期

王素芳，陈鹏，张维存，杨林

(1.中国航天科工集团第二研究院七〇六所，北京 100854；2.华北理工大学机械工程学院，河北唐山 063210；3.北京科技大学自动化学院，北京 100083)

0 引言

为解决服务型机器人在室内环境准确定位[1]，快速运动时图像易模糊的缺点[2,3]，本文采用了需要依靠IMU(inertial measurement unit)、惯性测量单元与单目视觉相融合的定位方法，即VIO视觉惯性里程计[4](visual-inertial odometry)。本文融合框架采用了紧耦合这种相对复杂但主流的研究方法。

英国Davision率先提出了使用通用运动模型进行平滑的相机运动以及单目视觉特征点预估计的MonoSLAM系统，极大提高了机器人在实时3D定位与地图绘制中的应用价值[5]。Mur-Artal等[6]提出了ORB-SLAM2系统，相比于其提出的ORB-SLAM系统，新系统优化了后端的IMU与单目视觉系统，使其可以进行更精确的轨迹规划[7]。

在视觉SLAM中，以位姿和路标来描述SLAM过程，图像中所谓的特征点就是路标，图像特征包括SIFT[8]、SURF[9]、ORB等。ORB改进了FAST没有方向性的缺陷，并通过二进制的BRIEF描述子，大大加快了图像特征提取的速度，达到了SLAM实时性的要求。因此本文采用了ORB特征点和特征提取与匹配方法。本文主要对当前视觉SLAM的后端优化部分进行了算法创新，在原有非线性优化算法基础上应用了改进的Dog-Leg算法，又设计了一种基于阻尼因子更新来计算信赖区域半径的方法，最后应用于YOBY机器人并在实际场景中进行了算法测试。

1 视觉SLAM系统

本文的视觉SLAM系统主要由传感器数据处理采集、前端视觉里程计优化、后端优化、回环检测、建图五大功能模块组成，如图1所示。

图1 SLAM整体流程

本文具体的系统流程如图2所示，第一模块中常用的传感器有相机(单目相机、双目相机、TOF深度相机)IMU(惯性测量单元)，激光雷达等。第二模块中的视觉里程计是根据几个相邻图像信息的相似性，通过相应的算法处理，粗略地估计出相机的运动，为后端提供初始值。在第三模块后端优化中，由于前端视觉里程计只能通过相邻图像估计出局部相机位姿，由此构建的增量式地图存在累计误差，这就需要后端优化来构建一个全局一致性的地图来提高地图和位姿估计的准确性。在优化方法中，扩展卡尔曼滤波算法是通过上一时刻的状态来估计当前时刻的状态，而非线性优化算法是把所有时刻的数据放在一起进行优化，因此非线性优化算法极大地提高了估计的精度。第四模块回环检测的作用就是判断机器人是否到过相同的位置，从而形成闭环，增加约束条件，减少后端优化的计算量和累计误差，纠正机器人位姿。最后是地图构建。SLAM系统可以根据应用场景与实际需求的不同，构建出2D栅格地图、2D拓扑地图、3D点云地图和3D网格地图等地图。

本文具体的系统流程如图2所示：

图2 本文系统流程

(1)数据预处理：相机为30 Hz的单目相机，采用ORB方法进行特征提取和匹配，利用二进制描述子进行特征标记；将IMU数据进行积分，得到当前时刻的位置、速度和旋转(PVQ)，同时计算预积分增量、雅克比矩阵和协方差项；

(2)初始化：首先利用运动恢复结构(SFM)估计滑窗内所有帧的位姿和路标点的位置，然后与IMU数据融合恢复真实的尺度信息，最后求解初始化参数(如重力、速度、陀螺仪偏置)；

(3)后端非线性优化：将视觉约束、IMU约束和边缘化的先验信息约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的PVQ和bias；

(4)回环检测：利用DBoW2每3个关键帧检测一帧，相当于是跳两帧做一次回环检测，当检测成功后进行重定位；

(5)全局位姿图优化：滑窗内的优化仅得到了滑窗内相机的位姿，且回环检测利用回环帧也只能纠正滑窗内的相机位姿，也就是只进行了局部位姿的优化，并没有对整个相机的位姿进行修正。鉴于地图轨迹全局的一致性考虑，相机位姿图优化除了局部优化以外再做一次全局的优化。

1.1 SLAM前端

SLAM前端也被叫作视觉里程计(VO)，VO的实现方法，按是否需要提取特征，分为特征点法和直接法两种。对于光流法来讲，前提假设时间是连续的，并且运动是微小的以及灰度不变等条件苛刻，当相机发生大尺度运动或者旋转时无法较好的追踪；本文采用的特征点法被认为是视觉里程计的主流方法，在运动过大时，只要匹配点还在图像内，就不会引起帧丢失的情况，相对于光流法有更好的鲁棒性。

FAST[11]关键点以检测图像局部像素的灰度值为主，假如图像中的某一像素灰度值和它邻近像素之间灰度值差异明显，说明这个像素点很可能就是角点。FAST关键点如图3所示，突出显示的正方形是在角点检测中使用的像素，p处的像素是候选角的中心，圆弧由虚线表示，穿过12个比像素p更亮或更暗且连续的像素，称之为FAST-12。首先检测图像中的FAST点，选取像素p，且p处的亮度为Ip。快速取一个参数，即中心像素p和围绕中心的圆环中的像素之间的阈值T。如果圆中存在一组N个连续的像素，它们的亮度都大于Ip+T，或者都小于Ip-T，选择N为12是因为它经过了大量实验验证，可以用来排除大量非角点。为了更加高效，可以添加一项预测试，测试仅检查1、5、9和13处的4个像素的亮度。如果p是一个角点，那么其中至少3个亮度大于Ip+T或小于Ip-T。如果都不成立，则p不能成为一个角点。然后，可以通过检查圆中的所有像素，将完整的分段测试标准应用于剩余的候选项。预测试快速排除了绝大多数非角点，角点检测的速度得以提高。

图3 FAST关键点

FAST及其变体是在与视觉功能(例如，并行跟踪和映射)相匹配的实时系统中查找关键点的方法。它必须有效并且可以检测到合理的拐角关键点，因此必须通过金字塔处理方法来匹配不同缩放倍率下的图像，从而实现尺度不变性。对于旋转，本文采用了一个简单但有效的角点定向度量方法，即灰度质心。灰度质心假定角点的灰度偏离中心，这个向量便可以被用来表示特征点的方向。具体步骤如下：

定义图像块的距为

(1)

式中：I(x,y)为点(x,y)处的灰度值，通过这些距便能求出图像块的质心

(2)

计算夹角求出具体的方向信息便可以得到FAST角点的旋转描述

θ=arctan(m01/m10)

(3)

通过对FAST角点添加尺度和旋转的描述，大大提升了ORB方法在不同图像之间的鲁棒性，因而又称改进的FAST为OrientedFAST。

1.2 后端优化

SLAM前端视觉惯性里程计构建的地图为增量式地图数据会受到各种未知噪声干扰，此时的运动方程和观测方程不是精确成立的，而同时定位与地图构建旨在构建一个精确、可靠、生命周期长的地图，因此，只有视觉前端是远远不够的。这就需要后端，将带有噪声的数据通过优化方法进行准确的状态估计，从而降低误差带来的影响。后端优化主要有基于扩展卡尔曼滤波(EKF)的滤波方法和基于图优化的非线性优化方法[12]。

根据文献[13]，考虑使用立体相机和单目相机的SLAM以及各种不同的场景和运动模式，非线性优化优于滤波，因为它提供了每单位时间内计算的最高精度。非线性优化算法在精度和鲁棒性方面表现出比KF算法极高的优越性[14]。卡尔曼滤波方法在此刻的状态和观测只与上一时刻有关，而忽略了之前可能有关的数据的影响，相当于优化只迭代了一次，也就是只考虑上一帧的影响。而非线性优化算法[15]使用的是之前全部的数据，考虑的是所有帧对系统的影响来求解状态估计问题[16]，因此本文后端采用非线性优化算法。

构建函数h，假设三维点云图是由多个观测点组成，设观测点一共有N个，用y1,…,yN来表示。其中传感器在xk位置上看到某个观测点yj时，产生了一个观测数据zk,j，对于观测方程来说，第k次观测

zk,j=h(yj,xk)+vk,j

(4)

观测数据的条件概率为

P(zk,i|xk,yi)=N(h(yj,xk),Qk,j)

(5)

因为噪声影响服从高斯分布v(k)～N(h(yj,xk),Qk,j)，因此条件概率仍然为高斯分布

(6)

对上式取负对数可得

(7)

将式(7)中与x无关的项去除并带入SLAM的观测方程中，求得状态的最大似然估计

(8)

(9)

得到了一个总体意义下的最小二乘问题

(10)

这个可以最小二乘问题表示为

(11)

假设对f(x)的第i个分量fi(x)在点xk处Taylor展开：fi(xk+h)≈fi(xk)+▽fi(xk)Th，i=1,2,…,n；可以得到f(xk+h)≈f(xk)+J(xk)h，其中Jacobin矩阵的计算公式为

(12)

1.3 回环检测

由于随着关键帧数量的不断增加，地图的规模也在不断地扩大，这时，最小二乘问题中的残差数据就会越来越多，信息矩阵也会越来越大，计算量将会随之加大。因此，为了保持计算的实时性，本文采用滑动窗口方法[17,18]来限制后端优化的计算时间，即保持优化变量的个数在一定的范围内，将其放在一个滑动的时间窗口中，动态地增加或丢弃优化变量。也就是说随着新的关键帧的加入，旧的关键帧将被丢弃，新的变量加入到最小二乘系统中进行优化。为了既丢弃掉旧的变量，又能够保留下被丢弃变量携带的信息，并将信息传递给剩余的变量，这里便采用了边际概率的做法来移除旧的变量。

在滑动窗口中，有相机位姿、IMU测量值和视觉测量值，它是滑动窗口内状态向量、IMU和视觉的紧耦合。将视觉约束、IMU约束和先验约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的PVQ和bias。目标函数为

(13)

式(13)中，第一部分为基于舒尔补得到的边缘化的先验信息，第二部分为IMU测量残差，第三部分为视觉重投影误差。

滑动窗口和边缘化方案虽然减少了计算量和计算复杂度，但同时也使系统漂移累积起来[19]，为了消除漂移，将重定位加入到紧耦合中。重定位从回环检测[20,21]模块开始，用BRIEF二进制描述子通过DBoW2[22,23]词袋模型进行循环检测，当识别到已经访问过的位置时，将回环帧添加到滑窗中进行优化，优化出的回环帧位置如果与之前窗口中的对应帧优化出的位姿值大于20的时候，则认为回环是错误的，剔除此回环帧。如果满足判断条件，则加入到滑窗中一起进行位姿优化

(14)

式(14)中的第四部分则是在回环检测中的预测值和测量值之间的误差。将回环帧的位姿视为常量，加入到滑窗进行紧耦合优化时，状态量的维度保持不变。由于IMU的加入，roll(滚转角)和pitch(俯仰角)变得可观，尺度信息在初始化的时候也已得到，因此最后使用回环帧和对应帧的关系进行四自由度全局位姿图优化，全局优化后得到的位姿准确度更高以及平滑性更好。

2 增强实时性的Dog-Leg算法

非线性优化问题的框架大致分为线搜索和信赖区域两大类。高斯牛顿法属于线搜索，即先确定搜索方向，再确定步长。而列文伯格-马夸尔特法属于信赖区域方法，即先确定区域，然后在区域内寻找最优，它认为近似只在区域内可靠。列文伯格-马夸尔特法是最速下降法和高斯牛顿法的混合，根据阻尼因子的不同在两者之间切换，Dog-Leg[24]方法同样是最速下降法和高斯牛顿法的混合，不同之处在于Dog-Leg方法把调整阻尼因子变为了改变信赖区域。

Dog-Leg方法[25]流程如图4所示。

图4 Dog-Leg方法流程

本文对Dog-Leg方法已有的更新方法进行改进，得到新的更新策略如图5所示。

图5 本文阻尼因子更新策略

通过MATLAB软件画出阻尼因子随着迭代变化的曲线图，从图6可以看出，原策略迭代次数多于本文所设计更新策略的迭代次数，当迭代至第7次时本文所设计的更新策略的阻尼因子值达到最大值，但远远小于原策略的最大值，在迭代至第9次时原策略阻尼因子值达到最大值，此时本文所用更新策略的阻尼因子已下降到很小的值。因此，本文所用更新策略要优于原更新策略。

图6 更新策略迭代次数对比

3 实验验证

3.1 基于标准数据集的测试与分析

在EuRoc MAV-MH_05公共数据集上运行和评测本文所设计的方法。EuRoc数据集是在微型飞行器上收集的视觉惯性数据集，该数据集包含双目图像、同步的IMU测量值以及三维位置的真值，着重于视觉惯性SLAM的评估测试。ORB-SLAM2与VINS-Mono都是开源的实时SLAM框架，本文SLAM系统与这两种SLAM框架的对比结果如图7与表1所示。

图7 本文算法误差

表1 相对位姿误差

表1中RPE表示相对位姿误差，mean表示平均值，median表示中值，rmse表示均方根误差，sse表示误差平方和，std表示标准差。图7(a)表示本文算法的轨迹误差，图7(b)为本文算法的相对位姿误差图。由表1可以看出，本文方法的RPE在0.0527～0.000 02之间波动。rmse对数据中的特大或者特小值敏感，可以很好地反映出轨迹的精度，sse是拟合数据和真值对应点的误差的平方和，rmse和sse越小,说明测量的数据精度越高，稳定性越好。在表1中，rmse大小关系为：rmse(ORB-SLAM2)>rmse(VINS-Mono)>rmse(本文的方法)，sse的大小关系为：sse(ORB-SLAM2)>sse(VINS-Mono)>sse(本文的方法)。验证本文算法具有较高的精度和较好的稳定性。

图8和图9分别展现了ORB-SLAM2、VINS-Mono和本文优化后SLAM算法的平移和旋转的细节描述。本文优化后的SLAM算法与ORB-SLAM2和VINS-Mono相比较，在平移和旋转上与真值之间的误差较小，拟合效果较好。

图8 全局轨迹对比

图9 六自由度轨迹对比

3.2 基于YOBY机器人实际场景的测试与分析

在本文的实验环境中，软件平台使用的系统为ROS机器人操作系统。硬件平台采用游尔机器人公司的底盘、国产Firefly-RK3399开发板和小觅相机等硬件搭建测试环境。

如图10(a)为机器人的三维结构图，图10(d)机器人的底盘由一个万向轮和两个驱动轮组成。底盘中央处理器为八核Cortex-A53(64 bit)架构CPU1.3GHz。头部配备图10(b)国产Firefly-RK3399控制板，作为Firefly新一代顶级平台，采用六核64位处理器，具备高性能数据传输和显示接口，支持2.4 Hz/5 Hz双频WiFi,Bluetooth和千兆以太网。安装了图10(e)小觅相机与LM2596S降压稳压电源模块，软件系统为Ubuntu16.04系统。