基于单目稀疏法多传感器融合移动机器人定位

王立玲，李森，马东

(1.河北大学电子信息工程学院，河北保定 071002；
2.河北省数字医疗工程重点实验室，河北保定 071002)

移动机器人在运动中自主导航完成设定任务，需要定位系统计算自身所在环境中的位姿。SLAM是同步定位与地图构建(Simultaneous Localization and Mapping)，主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。为获取先验信息确定移动机器人的位姿，在设计移动机器人时配备了各种传感器，如编码器、IMU(惯性测量单元)传感器、相机、激光雷达等。移动机器人从搭载的传感器上获取到观测数据，进行移动机器人相对于未知环境的位姿估计，完成定位。

移动机器人自主定位导航算法研究过程中，基于MEMS-IMU的捷联式惯性导航系统以其成本低、体积小、功耗低等成为研究的热点[1]。尤其，基于MEMS-IMU/GPS 的组合导航系统[2]被广泛用于各种领域，在移动机器人定位领域产生深远影响。针对GPS无法提供服务的地方，提出一种基于高精度惯导和里程计的车载 DR 误差补偿算法。但是，此系统定位误差会随时间迅速增长,无法满足实际使用要求[3]。

为了提高移动机器人姿态估计的精确性，采用多个传感器进行数据融合，完成移动机器人定位位姿计算。传感器融合的优点是能够提供比单个传感器更好的测量精度和更精准的推断。

文献[4]中提出了一个基于事件的卡尔曼滤波器的传感器融合框架，将全局传感器和惯性测量单元的测量结合，在基于事件的时间表上，改进了计算资源有限的移动机器人的定位系统。文献[5]中提出了一种融合GPS和IMU测量的自适应神经元模糊推理系统。

根据载体搭载的传感器不同，可构建不同的SLAM系统。当机器人搭载的传感器是相机，则视为视觉SLAM。其中，视觉SLAM分为单目视觉、RGB-D SLAM、双目SLAM。由于单目相机质量轻、功耗小、成本低，能够广泛应用在各个领域。MUR-ARTAL 等[6]于2015年提出了ORB-SLAM算法，运用单目相机在计算机的CPU上实时建图与定位。该算法的关键点提取和描述子的计算花费近20 ms；
但是，会忽略除特征点以外的大部分信息，在特征缺失地方采集不到足够的匹配点进行计算，从而可能丢弃有用的视觉信息。FORSTER等[7]于2014年提出了一种半直接法的视觉里程计SVO(Semi-direct Monocular Visual Odometry)，前期只跟踪稀疏的关键点(不计算描述子)，再使用直接法估计相机的位姿，计算速度快于使用特征点法的ORB-SLAM2，此算法存在累计误差，在特征点丢失后便不能进行重定位如果相机自动曝光，当调整曝光参数时，会使得整体变亮或变暗造成直接法无法计算。

为解决视觉SLAM系统在定位过程中遇到的问题，如单目SLAM系统无法恢复尺度信息等，香港科技大学QIN等[8]提出VINS-Mono(Monocular Visual-Inertial State Estimator)算法，通过单目视觉与IMU结合恢复单目相机的尺度因子，实现在四旋翼飞机载体中的定位与导航。但是，算法整体对单目相机和 IMU的性能以及CPU的数据处理要求较高[9-10]。随后，提出支持多种传感器组合的视觉惯性SLAM算法框架VINS-Fusion[11]。MUR-ARTAL、TARDS[12]提出结合ORB-SLAM 紧耦合的单目视觉惯性 SLAM，使单目相机的状态变量与IMU 传感器的状态变量进行数据融合，采用紧耦合方式共同构建出系统的运动方程和观测方程，再计算系统的状态。其算法中IMU传感器的状态变量的初始化依赖单目相机状态的初始化。

综合上述研究，本文作者在无视觉信息输入以及单目相机移动过快视觉跟踪丢失时，通过扩展卡尔曼滤波融合里程计和IMU输入观测数据进行移动机器人定位，提供短时间位姿估计，保证在缺失相机位姿时移动机器人能精确定位；
视觉前端相机位姿计算采用半直接法，在移动端侧(嵌入式端)上实时运行；
移动过程中完成单目相机尺度信息的计算，单目输出位置信息通过编码器输出的速度信息和IMU信息进行扩展卡尔曼融合输出位姿，改善了编码器和惯性测量单元单独定位精度低、累计误差大的缺点；
边缘侧混合式多传感器融合免去当前移动机器人移动过程必须有线连接便携式计算机或者将便携式计算机放置在移动机器人顶部进行SLAM算法计算的方式，减小了移动机器人的体积，具有广泛的扩展性。

MUR-ARTAL等提出的ORB-SLAM系统与其他SLAM系统相比较具有较高的精度和运行效率。但是，嵌入式领域不具备实时运行ORB-SLAM系统的能力。半直接法(SVO)视觉SLAM算法能够在移动侧运行完成相机位姿估计，但是半直接法(SVO)视觉里程计只能估计当前时间的相机位姿，不具备后端优化和闭环检测的能力。

作者采用特征点与直接法相结合的方法进行优化相机位姿，将半直接法(SVO)前端位姿计算方法融合到ORB-SLAM2中，充分利用直接法对光照变化、稀疏纹理的鲁棒性，在使用单目相机之前首先进行相机标定，获取相机的内参以及畸变参数。融合后的半直接法的初始化位姿流程如图1所示。

图1 Tracking线程流程

最小化残差损失函数：

(1)

通过式(1)计算帧间匹配得到的当前帧相机位姿具有累计误差，从而导致相机位姿漂移，因此需要通过特征对齐找到地图中特征块在新一帧中真正的位置。基于光度不变性的假设，重新构造残差对特征点的预测位置进行再次的优化：

(2)

式(2)优化后比式(1)预测的相机位姿位置更精准，但是式(2)优化后和式(1)预测的特征点的位置也会存在误差，因此需要构造新的目标函数进行优化。由式(3)进行优化计算，得出更准确的相机位姿：

(3)

张福斌、刘宗伟[13]提出改进ORB-SLAM2算法，将单目相机、三轴陀螺仪、里程计3种传感器进行紧组合，通过非线性优化实现精度较高的定位。而文献[14]中创新性是提出稳定自治系统的光照鲁棒直接视觉里程计，空中机器人在不可预知的光照条件下飞行。因此，为提高移动机器人的定位精度采用多传感器融合方法。

根据传感器融合方法的不同，多传感器融合方式分为松耦合和紧耦合两种方式。其中，松耦合是各个传感器以各自的传感器数据进行运算后再以融合算法将其数据融合后计算出位姿。紧耦合是考虑各个传感器之间的耦合性，通过视觉和惯性传感器进行数据耦合计算，但是对计算载体的计算算力具备一定要求。多传感器紧耦合框架面对某个传感器失效时，会出现迅速漂移等问题。松耦合面对单个传感器故障而导致失效时，也能够精确定位。因此，本文作者采用基于扩展卡尔曼滤波的松耦合融合算法进行定位研究。

2.1 算法思路

本文作者通过采集单目摄像头数据流、IMU和里程计数据，通过扩展卡尔曼滤波融合算法进行位姿定位。基本思路如图2所示。

图2 算法思路

2.2 单目视觉位姿估计模型：

针对单目相机，选用针孔相机模型。

Puv=KPw

(4)

(5)

移动机器人移动的距离xb通过编码器和IMU的扩展卡尔曼融合数据获取，解决单独使用编码器计算移动距离导致的误差累计这一问题。与此同时获取固定时间间隔内的单目视觉里程计估算的移动距离xc。在实验过程中，实时估计计算出单目视觉的尺度因子，采用如式(6)进行计算，完成单目视觉的尺度恢复。

λ≈xb/xc

(6)

2.3 里程计模型

2.4 IMU运动模型

IMU的观测模型描述如式(7)—式(8)所示，上标I表示IMU坐标系，上标G表示参考坐标系。

(7)

(8)

以扩展卡尔曼滤波法作为多传感器融合框架，是以松耦合方式融合编码器里程计速度、单目视觉位姿、惯性传感器信息进行定位。设计的SLAM系统算法框架如图3所示。

图3 多信息融合算法

由于移动机器人工作在平面中，它是非完整约束机器人，需要将3D变量归零，包括Z轴、俯仰、横滚以及各自的速度和Z轴加速度。但是需要将Y轴速度融合进去，因为只有这样才能够证明Y轴没有移动。即表示为7维列向量：

系统方程及测量方程：

xk=f(xk-1)+ωk

(9)

zk=h(xk)+vk

(10)

其中：变量xk、zk分别表示为在时间k时刻的状态向量和测量向量。协方差为Q的过程噪声由变量ωk-1表示，协方差为R的观测噪声由vk表示。过程噪声和观测噪声为互不相关的零均值高斯白噪声。

(11)

(12)

(1)系统状态初始化

多传感器融合系统在接收到传感器发出的第一个测量数据时，对系统状态变量和状态协方差进行初始化，得出初始状态量和初始状态协方差。

(2)预测过程

(13)

(14)

(3)更新过程

(15)

(16)

(17)

上述传感器的输入数据Rk受到误差Q和方差Pk-1的影响。

4.1 实验平台

为完成上述实验算法的验证，采用自主设计实验平台，验证平台采用Ackerman模型的四轮智能小车移动机器人，按图4所示的硬件框图自主搭建实验平台。设计的移动机器人采用Teensy单片机读取直流减速电机和MPU9250数据，视觉里程计传感器采用树莓派自带单目相机。多传感器融合的决策部分采用树莓派完成移动机器人位姿估计计算，PC端实时显示当前位姿，通过路由进行无线链接进行数据传输。搭建完成的实物平台如图5所示。

图4 硬件系统框图

图5 实物平台

硬件平台参数：

(1)相机为Pi Camera V2：分辨率2 592像素×1 944 像素(500万像素)，感光芯片OV5647,角度65°；

(2)惯性传感器采用MPU9250，其中陀螺仪采用±250(°)/s，陀螺仪的RMS噪声0.1(°)/s(RMS),噪声密度0.01(°)/(s·Hz1/2)；
加速度为±2g；

(3)采用减速比30、编码器精度为390的直流减速电机；

(4)采用四核64位处理器的3B+树莓派；

(5)计算机：RedmiBook16,CPU为AMD-R7-4700U；

软件平台：

开发平台使用安装Linux系统虚拟机的RedmiBook16便携式计算机。Linux系统版本是Ubuntu16.04，并在此系统下安装ROS机器人操作系统。树莓派使用Ubuntu16.04系统并搭建ROS系统。运用ROS分布式的系统优势，进行数据传输完成算法的软件开发。

4.2 实验设计

采用Nature-Point公司的高速动作捕捉系统Motive，完整记录移动机器人顶部mark点的移动轨迹，设定动作捕捉系统记录的移动轨迹为真值。设定小车以约0.01 m/s的速度恒速运行，前轮舵机以约0.012 rad/s速度进行转弯。其中，由于移动机器人运动在平面上，只有绕Z轴转动，因此绘制XY轴上的轨迹。图形以EKF融合后的数据以及动作捕捉获取的移动小车的真实位置数据绘制。

为验证光照变化对单目视觉以及融合算法的影响，根据不同时间光照射入室内，在室内形成的光影变化不同，设定光照较为明亮的14：00-16：30时间段为时间段1和光照变化明显的17：00-19：30时间段为时间段2，在两个时间段进行验证实验数据采集。

设定两种运行轨迹运行方式：

(1)圆弧运行：(1)以设定转向角度前进运动轨迹成半圆弧；
(2)先以设定的舵机转向角度前进，然后再以原点为前进方向运行，后形成椭圆形圆弧。

(2)直线加圆弧运行。进行直线运行，完成两次直角转弯，最后采用圆弧转弯回归到起始位置。

上述两种运行轨迹能够完成室内移动机器人室内移动、避障、返回充电等全部运动的原始基本操作。

4.3 实验结果

图6是在时间段1移动小车以设定转弯角度在轴方向运行约1 m左右后，再以原点作为运行终点，记运行的为轨迹1。

图6 轨迹1实验结果对比

图7是在时间段2先沿直线运行，在第二个直角转弯后，以设定转动角度使移动小车返回到起点，记为轨迹2。

图7 轨迹2实验结果对比

表1 不同传感器及融合方案均方根误差比较 单位：m

在光照强度变化的场景下，单目视觉里程计误差明显减小，单目稀疏法的视觉里程计能够准确估计相机位姿。其中，扩展卡尔曼滤波融合各个传感器数据后得到的均方根误差相较于单个传感器均方根误差明显减小。

以上实验证明在光照强度变化的环境中采用稀疏直接法(半直接法)的视觉里程计计算出的相机位姿具备有效性和准确性，并且根据视觉得出的位姿，EKF融合算法抑制了编码器的误差累计，有效提升整体移动机器人的定位精度。

以稀疏直接法作为单目视觉的前端估计相机位姿，结合惯性传感器和编码器进行扩展卡尔曼滤波多传感器融合定位。通过在光照明亮不变和光照变化的时间段内进行实验,验证此算法的准确性，证明该算法能够改善在光照变化的场景中的定位精度，提高单个传感器定位的定位精度，降低编码器的累计误差，扩展单目视觉的应用场景，减小了当前自主定位移动机器人的体积。