IMU松耦合的KinectFusion算法改进

应宏钟，高宏力，宋兴国，杨林志

（西南交通大学，四川 成都 610031）

1 引言

在未知环境中实现移动机器人的增量式地图构建（Mapping）与实时定位（Localization），是移动机器人智能化不可缺少的重要核心技术，这两项技术被统称为同时定位与地图构建SLAM（Simultaneous Localization and Mapping），是移动机器人领域热门的研究方向之一。

SLAM最早在1987年由R.Smith等人提出，创造了SLAM的第一个分类—基于扩展卡尔曼滤波器的EKF-SLAM[1]。而后文献[2]使用超声波传感器构建了第一套移动机器人SLAM系统。而后随着激光雷达的出现，SLAM系统的实用性得到了极大的提升，文献[3]利用激光雷达制作了一套十分优秀的无人机室内导航SLAM系统，但是具有较高的使用成本。

近年来，随着计算机性能的快速提升，基于机器视觉的VSLAM获得了巨大的成功，目前主要有三类V-SLAM：单目相机SLAM（Mono SLAM）、双目相机SLAM（Stereo Vision SLAM）以及RGB-D SLAM。单目相机SLAM只需要单个摄像头就能实现成本极低，但是只能获取相对深度，然后在相似变换空间中求解，不能在传统的欧式空间中求解，存在尺度漂移问题[4]。双目相机SLAM可以通过多个相机估计点的位置，且不受运动的影响，但是带来的是较大的计算量，实时性能较差。RGB-D SLAM是通过RGB-D相机实现的，相机在采集RGB信息的同时通过Time-of-Flight原理或者结构光直接获取各个像素点的深度信息，以较低的成本和较小的计算量获取环境信息，其中较为经典的是KinectFusion[5]算法。该算法通过Kinect深度摄像头获取RGB图像和深度数据，调用CUDA对数据进行多线程处理，具有较高的是实时性，并且首次实现了实时稠密的三维地图重建与自身定位。但是存在ICP（Iterative Closest Point）定位失效的问题，容易丢失摄像头的位姿变换信息。通过加入惯性传感器IMU来获取摄像头的运动状态，解决ICP定位失效的问题，提升算法的鲁棒性和定位精度。

2 KinectFusion算法描述

2.1 Kinect摄像头

Kinect摄像头主要由红外激光投射器、红外相机、RGB相机组成，其采用的深度获取原理Light Coding为：由红外激光投射器投射出经过“体编码”的激光散斑（Laser Speckle），散斑在不同距离有着不同的结构形式，投射在物体表面时形成独特唯一的形状结构，再由红外相机捕捉这些物体表面的光斑结构，解算出深度信息。

Kinect摄像头可以获取深度范围为（0.4～3.5）m，水平检测范围 57.5°，垂直检测范围 43.5°，3m 处的距离误差为（±3.5）cm，分辨率为3cm[6]。

2.2 KinectFusion算法

KinectFusion算法的基本流程，如图1所示。

图1 KinectFusion算法基本流程Fig.1 Flow Chart of KinectFusion Algorithm

主要包括四个部分：

（1）深度数据处理：使用双边滤波（Bilateral Filtering）对原始深度数据进行处理，在保留边缘的同时对边缘进行平滑。用深度图像帧中每个像素点u=（x，y）的深度值Di（u）以及相机内部矩阵K来计算顶点向量Pi（u），再通过Pi（u）计算每个点的法向量ni（u），将深度图像中带深度值的二维像素点转化为以相机为原点坐标系中的三维点；

（2）相机跟踪：使用 ICP（Iterative Closest Point）算法，对相机进行跟踪，通过当前帧的点云与已有模型点云之间的匹配，求解出相机的相对位移与转角；

（3）体积融合：使用 TSDF（Truncated Signed Distance Function）方法实现体积融合，将得到的点云数据融合到网格模型中。在网格模型中共有n3个栅格单元被称为体素，每个体素中存有一个介于［-1，1］之间的值，该值表示了体素到测量面的距离，当值为正时表示体素在测量面的后面即在物体内部，反之当值为负时表示体素在测量面的前面，即物体外部。每当有新的数据需要融合进网格模型时，通过式（2）对网格模型进行更新，当场景变化快或者有新场景出现时会给出较大的权值，以快速的更新网格模型，当场景较为稳定时会给出较小的权值，来减小噪声对模型的影响。

式中：TSDFavg—加权融合后的网格模型；TSDFi-1—原有的网格模型；TSDFi—新增的网格模型；wi—权值。

（4）地图渲染：使用光线投影算法对网格模型进行渲染，将模型可视化的同时，渲染出的模型点云可以用于下一帧输入深度图的ICP配准，就此完成一个循环。

3 IMU松耦合的ICP算法

3.1 ICP算法

在KinectFusion中点云与已有模型之间的匹配是一个3D-3D的位姿估计问题，采用投影法来计算每帧画面与模型之间匹配点的位置变化。在相机坐标系中Ps（u）为K时刻待匹配的原始点云，Pd（u）为K-1时刻融合成的TSDF模型上的目标点云，ni（u）为目标点云Pd（u）的单位法向量，在ICP算法中使用如下目标函数求解位姿变换，如图2所示。

上式构建了一个最小二乘问题，当目标函数E取得最小值时，式中的矩阵M即为所求的位姿变换矩阵，且矩阵M由一个旋转矩阵 R（α，β，γ）和一个平移矩阵 T（tx，ty，tz）组成。

图2 模型与点云匹配示意图Fig.2 Schematic Diagram of Model and Point Cloud Matching

在实际程序中，由于深度摄像头采样频率为30帧每秒，两帧之间的变化较小，所以可以通过线性化的方式来求解，做以下近似：sinθ≈θ，cosθ≈1，将位姿变换矩阵 M 简化为Mˆ=T（tx，ty，tz），原目标函数就可以通过线性方程组（4）来求得最优解。

在实际测试中，上述的ICP算法经常会定位失效，当Kinect相机快速移动时，K+1时刻的深度帧与K时刻已有模型之间的位置变化较大，能匹配的点较少，直接使用ICP进行最近点迭代容易得到错误的位姿变换矩阵，甚至无法求得位姿变换矩阵。当相机视野中大部分区域为平面时，有可能无法求得正确的与平面平行的两个坐标轴方向上的位姿变换，造成建模中断或者建模错乱。目前已有的解决方法有通过边线关系的ICP算法改进[8]，结合RGB信息进行位姿估计[9-10]等。

3.2 IMU松耦合的ICP算法改进

IMU惯性传感器能够测得角加速度和加速度且具有较高的采样频率，当相机运动过快或者场景中存在大平面区域导致ICP算法失效时，可以通过IMU采集到的数据进行位姿估计，弥补纯视觉SLAM产生的缺陷。

IMU惯性传感器由三轴陀螺仪、三轴加速度计和三轴磁力计组成，在测量时能直接测得角速度、线加速度和磁偏角。理论上对角速度进行积分就能获取转过的角度，对加速度进行二次积分就能获取位移，但实际使用中传感器存在漂移、噪声和测量误差，需要使用互补滤波（CF）、扩展卡尔曼滤波（EKF）等算法，对采集到的数据进行滤波和数据融合，减小数据噪声和测量误差造成的影响，提升传感器精度。

3.2.1 计算姿态四元数与位移估计

通过较为成熟的Madgwick算法[7]来实现姿态估计，该算法属于互补滤波算法，使用四元数微分方程和测得的角速度来求解姿态，然后利用地磁计和加速度计通过梯度下降算法进行补偿。在位移估计时，利用已经求解的姿态将三轴加速度中的重力加速度分量去除，然后进行二次积分得到位移向量。由于长时间内的位移估计存在极大的累积误差并且难以消除，所以只使用短时间的位移估计。将所用IMU的采样频率设置为150Hz，相机获取深度帧的频率为30Hz，每五次IMU采样时间等同于一次深度摄像头的采样时间，具体描述如下：

式中：qest，t—t时刻的姿态四元数；qˆest，t-1—t-1 时刻的姿态四元数；q˙ω，t—由测得的角速度计算得到的姿态四元数的变化率；β—陀螺仪的测量误差；fg，m（qˆ，aˆ，mˆ）—加速度和磁偏角相关的多元向量误差函数；Δt—IMU的采样时间间隔；at—测得的三轴加速度向量；Rq，t—t时刻求得的由姿态四元数表示的旋转矩阵；g—重力加速度向量；TK—五个采样周期内的位移向量。

3.2.2 坐标系变换

IMU传感器与Kinect相机的相对位置固定不变，存在一个固定不变的坐标系变换矩阵，将IMU测得的位姿变换转化到相机上。公式如下：

3.2.3 数据融合

使用最优加权融合对ICP算法和IMU测量得到的位姿变换矩阵进行融合，记由IMU得到的位姿估计方差为，ICP 算法计算得到的位姿估计方差为，对应的加权系数分别为w1和w2，为使融合后的方差最小，构造辅助函数如下：

4 实验

在室内场景中对所述算法进行实际测试，所用的实验设备，如图3所示。由一个Kinect摄像头和一块9轴IMU传感器组成。所用程序在PCL开源库的基础上进行开发，运行环境为ubuntu14.04操作系统，Intel i7 4790K CPU，16G 内存，NVIDIA GTX970显卡4G显存。

图3 摄像头和IMU实物Fig.3 Camera and IMU Suit

实际实验所使用的参数，如表1所示。在实际实验中IMU所得位姿变换矩阵的标准差由IMU传感器的标准差来近似，取σ1=0.0992.σ2用 σ2=σcamek（Δθ/θ+ΔT/T）计算得出，其中 σcam为 Kinect深度传感器的初始标准差参考文献[6]取σcam=0.0233，k为自行给定的放大系数，取k=20，Δθ和ΔT为ICP算法与IMU求解出的三轴转动角度和位移的差值，θ和T分别为IMU求解出的三轴转动角度和位移。

当ICP算法和IMU求解的位姿相近时，以ICP算法求解的位姿为主，减小IMU漂移产生的误差，如图4所示。当快速移动或者场景匹配像素点较少时，ICP算法误差增大甚至失效，两个算法得到的位姿估计差值增大，降低ICP算法求解结果的加权系数，当差值超过20%后，舍弃ICP求解的结果。

表1 实验参数Tab.1 Experiment Parameters

图4 标准差σFig.4 Standard Deviation σ

改进后算法对室内场景的重建结果，如图5所示。原算法运行结果，如图6所示。当相机视角从下往上移动通过平整墙面时，改进后算法顺利通过并完成了对部分天花板的地图重构，没有明显形变；原算法在通过平整墙面时由于大部分区域为平面，求解出了错误的位姿估计，最后地图重建和定位都发生了错乱。

图5 改进算法重建结果Fig.5 Map of Improved Algorithm

图6 原算法重建结果Fig.6 Map of Original Algorithm

5 结论

使用惯性测量传感器IMU对较为经典的实时稠密RGB-D SLAM算法—KinectFusion算法做了改进。使用IMU与ICP算法松耦合实现最终的位姿估计，解决了快速移动或有效配准点稀少时造成的ICP算法失效问题。通过实验证明了，改进算法在鲁棒性优于原有算法的情况下，解决了原有算法存在的问题，且没有明显的实时性问题，能顺利的实时完成地图重构与定位。