基于双目视觉SLAM 的室内机器人导航技术

梁明亮* 王云飞 张 凯

（郑州铁路职业技术学院，河南 郑州 451460）

双目视觉SLAM 是在传统SLAM 基础上发展而来的，由于使用了双目相机因此更容易获得物体的三维信息，在此基础上利用特征匹配等技术创建三维点云地图，从而可以重现机器人周围环境信息，为精确定位提供了技术支持。在生成的三维点云地图上，使用PRT（双向路径）算法或PRM（概率路标图）算法，可以自动规划出移动路径，进而实现机器人的自主导航。

1 基于双目视觉的SLAM 定位技术

1.1 视觉SLAM 定位流程

SLAM 技术的核心作用是在未知环境下实现对移动机器人的精确定位，并对机器人所处环境进行三维重建。标准化的双目视觉SLAM 流程大体可分为4 个模块，即传感器数据采集模块、视觉里程计算模块、后端优化模块和闭环检测模块，最后输出待测环境的三维地图，整个流程如图1 所示。

图1 视觉SLAM 定位流程图

结合图1，各个模块的基本功能如下：（1）传感器数据模块。利用安装在室内机器人上的双目相机，可以获取机器人周围一定范围内的图像信息。如果机器人上安装有定位传感器，还能获得环境范围内物体的三维坐标。（2）视觉里程计。通过估算相邻图像间的相同特征确定相机的运动。（3）后端优化。在接收传感器采集数据的基础上，根据数据获取时间估算相机位姿，以及机器人的回环情况。在信息优化的基础上，可以得到全局一致的轨迹和地图。（4）回环检测。将当前图像与以往图像进行对比，可以判断机器人是否移动到指定的位置。将判断结果重新返回给后端。如果判断结果为“是”，则机器人在当前位置待机，等待下一指令；如果判断结果为“否”，则重新调整轨迹，让机器人继续移动直到到达指定位置。最后根据相机轨迹和提取的三维点云建立地图。

1.2 常用的视觉SLAM 定位算法

1.2.1 基于卡尔曼滤波的SLAM 算法

卡尔曼滤波（EKF）算法可用于解决系统过程中的状态最优估计问题，在机器人控制方面应用广泛。它可以将非线性系统的状态方程和测量方程，用泰勒级数展开求偏导，从而达到线性优化的目的。基于卡尔曼滤波的视觉SLAM 算法流程如图2 所示。

图2 基于卡尔曼滤波的SLAM 流程图

对非线性系统进行数学建模是基于卡尔曼滤波的SLAM 算法的关键步骤。其中非线性系统的状态方程和测量方程为：

上式中，Q 和R 分别为状态噪声和观测噪声的协方差矩阵。但是在实际中，测得2 种噪声值的难度较大，一般选择忽略。将式（1）和式（2）转化后，可以得到关于非线性系统的f 函数和h 函数。然后使用泰勒级数将这2个函数展开，然后分别求偏导数，即可将非线性系统问题线性化。

1.2.2 基于关键帧的SLAM 算法

结合上文分析可知，在卡尔曼滤波算法中，系统在k时刻的状态只与k-1 时刻的状态有关，而k-1 之前的状态结果和观测结果均不会对k 时刻的系统产生影响，这就导致卡尔曼滤波下的SLAM 定位可能存在非线性误差。基于关键帧（BA）的视觉SLAM 算法可以将优化相机位姿矩阵的问题转化成为求解非线性最小二乘的问题。根据最小二乘法求解处机器人的路标位置信息，在路标位置和机器人所处位置之间规划路径，从而实现精确定位。基于关键帧的SLAM 算法流程为：

定义一个状态向量x，将需要估计的变量放在状态变量集合中，该集合可表示为：

上式中J 为误差函数在线性点x0的雅可比矩阵，bk表示线性点处的残差。利用最小二乘法求出δxk后，即可得到精确的位姿信息，从而解决了卡尔曼滤波非线性误差的问题。

1.3 两种视觉SLAM 算法的仿真对比

在Matlab 仿真平台上分别对基于卡尔曼滤波的SLAM 算法和基于关键帧的SLAM 算法进行了仿真验证，并对比两种算法下的机器人定位精度。首先在软件中创建一个仿真环境，并随机摆放15 个路标。用菱形表示真实的路标位置，然后自定义一条真实的机器人移动轨迹。

基于卡尔曼滤波的SLAM 算法处理为：在Matlab 中分别建立移动机器人的运动方程和观测方程，使用该算法求解后，绘制出优化路径。同时根据卡尔曼滤波原理对路标位置重新定位；基于关键帧的SLAM 算法处理流程基本一致，仿真实验结果如图3 所示。

图3 视觉SLAM 仿真结果

结合图3 可知，通过卡尔曼滤波的SLAM 算法轨迹与实际轨迹偏差较大。其中在仿真机器人运行至第40s时，有最大轨迹误差，误差值为22.4mm；通过关键帧滤波的SLAM 算法轨迹与实际轨迹较为吻合，其中在仿真机器人运行至第9s 时有最大轨迹误差，误差值为10.7mm。

2 基于双面视觉SLAM 的机器人导航设计

2.1 构建点云地图

常用的点云地图有稀疏、半稠密和稠密3 种，由于室内机器人的活动范围较小，因此本文的机器人导航设计中选择了稀疏点云地图，既可以满足机器人的导航需求，同时还能减轻数据处理压力。稀疏点云地图的构建方法如图4 所示。

图4 稀疏点云地图的构建流程图

结合图4，系统开始构建点云地图前，首先会执行一个判断程序，即“是否插入新的关键帧”。如果判断结果为“是”，则直接插入一个新的关键帧，并且需要满足多个基本条件，如Kt帧能够检测出50 个以上的特征点；Kt帧与关键帧之间至少相差20 帧图像等。满足上述条件后，即可将关键帧插入并得到新的地图点云。如果判断结果为“否”，则需要重新检查地图回环，并对地图数据做优化处理。在双目相机模型中生成新的地图点云后，还要进行筛选。

2.2 构建栅格地图

栅格地图本质上是离散化的二值网格地图，它分为“空闲”和“占用”两种状态。若判断结果为“占用”，说明栅格地图的该处存在障碍，则机器人不得通过；反之，若为“空闲”，则机器人可正常通过。基于双目视觉的栅格地图构建流程如图5 所示。

图5 栅格地图的构建流程图

2.3 移动机器人路径规划

在室内机器人路径规划时，主要解决两个问题，即如何让机器人从初始位置移动到目标位置，以及在移动过程中顺利避开障碍。需要同时满足上述两项要求，才能保证机器人顺利完成既定的任务。本文选择最常用的路径规划算法A*算法，其流程如图6 所示。

图6 机器人路径规划算法流程图

结合图6，基于A*算法的机器人路径规划步骤主要为：（1）分别建立OPEN 表和CLOSE 表。其中，在OPEN表中主要添加待处理的节点，而CLOSE 表中主要包含已经处理完毕的节点。（2）以节点A 作为起始节点，将其加入到OPEN 表中。然后寻找节点A 周围其他的节点。并判断这些节点是否可以达到。同时执行一个判断程序，即“OPEN 表是否为空”，若该表为空，则将可以达到的节点放入到OPEN 表中。（3）通过代价函数F(n)计算出最小节点i，将其从OPEN 表转入CLOSE 表。同时检索所有与i 相邻的节点，判断是否有新的路径，以及新路径是否比刚才选择的值更低。将路径值最低的节点作为目标路径上的节点。完成所有节点的检索和判断后，即可规划出最优路径。

3 基于双面视觉SLAM 的机器人导航实验

3.1 特征点移动匹配实验

在室内机器人行进及转弯时，随机采集相邻关键帧进行ORB 特征匹配实验。在实验中发现，基于关键帧的SLAM 定位算法可以将无法匹配的特征点剔除，但是个别情况下还是存在无匹配的情况。分析其原因可能与两幅关键帧之间视角差偏大有关。随后在实验中将第6 和第7 两幅关键帧的视角差，从原来的60°减小到了45°，再次进行特征点移动匹配实验，结果发现机器人的SLAM 系统能够很好地匹配ORB 特征点，为机器人精确导航提供了有利条件。

3.2 路径规划实验

在构建的栅格地图上开展室内机器人路径规划实验。实验步骤为：首先启动室内移动机器人操作系统中的3D 可视化工具Rviz，等待栅格地图加载完毕。此时可以在栅格地图中观察到机器人模型的初始位置。然后自定义一个目标点，启动机器人自动导航系统后，机器人可以自动识别当前环境地图，确定自己的初始位置，然后通过SLAM 算法规划出从初始位置到目标位置的路径。

结束语

智能移动机器人的广泛使用，实现了劳动力的极大解放。但是在一些特殊作业环境下，对机器人的导航精度提出了严格要求，必须要对室内机器人的导航算法和寻路方式进行优化。本文设计的一种基于双目视觉的SLAM 室内机器人，融合了关键帧算法和A*算法，保证了机器人定位精确，可自动选择最优路径，能够较好地满足机器人的功能需求。