基于RGB-D相机和激光雷达的传感器数据融合算法

孙 健，刘隆辉，李 智，杨佳玉，申 奥

（湖南工程学院 电气与信息工程学院，湘潭 411104）

0 引言

传感器作为机器人环境感知的基础［1］，主要分为非视觉器件和视觉器件.非视觉器件（如激光雷达、超声波或红外传感器）具有抗干扰能力强、可全天候工作的优点，但该类传感器不足之处是采集的数据单一，且价格昂贵；视觉器件（如单目相机、双目相机和RGB-D相机）虽然成本更低，信息更丰富，但受环境影响大，检测精度低，视野范围有限.因单一传感器数据无法满足机器人不同场景的应用需求，传感器数据融合技术应运而生，并成为当前机器人技术的研究焦点，如图1所示.

图1 视觉激光融合示意图

近年随着技术不断的迭代更新与积累，目前已有许多关于传感器数据融合算法的研究.文献［1］通过EKF算法，将激光雷达信息和深度相机信息融合，但因其只对固定高度范围内的像素进行转换，缩小了RGB-D相机的感知范围，不能根据机器人所处实际环境自适应剔除地面点云.文献［2］先获取RGB-D相机点云中的地面点云，剔除后进行投影，再通过ICP算法将RGB-D相机点云与激光雷达点云配准融合，但算法精度和时效性较低，导致两传感器之间的位姿关系不能实时准确更新，影响数据融合效果.

针对上述问题，本文在对RGB-D相机获取的点云数据优化之后，利用PL-ICP点云配准实现数据融合，然后基于扩展卡尔曼滤波（EKF）进行精确融合，进一步提高融合数据精准度.

1 传感器模型

为更好地研究传感器数据融合的过程，使算法更具客观性和可重复性，本文对核心器件激光雷达和RGB-D相机进行建模.

1.1 RGB-D相机模型

RGB-D相机是一种应用较广泛的视觉器件，不仅可获取环境的图像信息，还可直接获取环境对应点的深度信息［3］，相比传统的双目相机依靠视差获取深度信息的方式，RGB-D相机大大地降低了计算的复杂度.

本文选用的RGB-D相机为Kinect v1［4］，如图2所示，硬件结构主要包括外壳、底座、散热器、4个麦克风阵列、传动电动机以及3个摄像头从左到右分别为红外投影机、颜色摄像机、红外摄像机，表1是Kinect v1的主要参数.

图2 Kinect v1的实物图

表1 Kinect v1主要参数

Kinect v1所获得的彩色图像和深度图像用点云来描述，其中每一个点均由r，g，b，x，y，z共6个分量分别表示该点的颜色与空间位置.

针孔相机模型如图3所示，对任意给定的一个深度图像点m()u,v,d，其对应的相机世界坐标点M(x,y,z)，可通过下式进行转换：

图3 针孔相机模型

其中fx、fy为相机在x、y两个轴上的焦距，cx、cy为相机的光圈中心，s为深度图的缩放因子.

为获得更好的成像效果，在相机前安装了透镜，由于其对光线传播产生影响，相应的也影响了相机坐标系与像素平面的一一对应.为消除这种影响，需对畸变进行矫正.畸变可分为径向畸变和切向畸变，通常影响作用较大的为径向畸变，下面主要分析径向畸变的矫正.

径向畸变随着与中心距离的增加而增加，可以用以下多项式函数描述：

式中：[x,y]T为归一化平面的点的坐标，[xdt,ydt]T为畸变纠正后点的坐标.对于像素平面中的某点，可以通过这些畸变系数k1、k2、k3得到该点在相机坐标系上的正确坐标.

1.2 激光雷达模型

激光雷达能提供环境精确的测量信息，有较广的感知范围，是移动机器人环境感知和构建地图的核心器件之一［5］.图4是本文所使用的激光雷达LS01B实物图，表2为其主要参数.

图4 LS01B实物图

表2 LS01B主要参数

LS01B采用三角测量法来获取环境信息.三角测量原理如图5所示，激光器向外发射激光，线性CCD接收障碍物反射回来的光线，根据反射原理，物体在CCD上的成像与物体和雷达之间的距离相关，因此在CCD上能够显示不同距离的物体的反射光.而激光器和探测器之间有一定间隔，根据三角测量公式，能计算出被测物体的距离.

图5 三角测量原理示意图

如图6所示，激光雷达获取的激光束与YL轴正向夹角为α，激光雷达坐标原点到障碍物点的距离为r.则激光测距点二元组()α,r与障碍物坐标(xL,yL)有如下转换关系：

图6 测距二元组与障碍物坐标关系图

2 深度相机模拟激光数据

根据上述激光雷达和RGB-D相机的模型，两传感器获取的环境信息的表现形式和实际意义都不同.为实现视觉激光数据融合，本文的策略是先使用深度相机模拟激光数据，然后将模拟激光数据与激光传感器数据融合.

2.1 点云优化

RGB-D相机模型可将深度图像转换为点云，但RGB-D相机采集的点云数据中包含地面点云.地面点云不属于机器人建图时的障碍物信息，在点云投影时会作为障碍物引入，因此需将地面点云提取剔除.

本文针对随机抽样一致性算法［6］（RANSAC）在地面起伏不平的复杂场景下提取地面点云不完整的缺点，先采用改进的RANSAC平面提取算法进行地面粗提取，而后利用法向量差分特征（DoN）［7］对剩余点云进行二次提取，达到提高地面点云提取完整的目的.具体步骤如下：

（1）原始点云数据利用体素化网格进行降采样滤波，在尽可能保持采样前后点云一致性的基础上，降低算法内存占用和提高算法时效性.

（2）计算地面点云数据法向量.

（3）估计地面点云平面方程，实现地面点云的粗提取.

由地面法向量计算地面的拟合方程：

利用RANSAC算法随机选择数据点并计算该点所在的平面方程π(A,B,C,D)，计算点云数据中每个点到该平面的距离，利用距离阈值dmax统计出该阈值内的点数Nin.重复随机采样更新平面方程，最终选择点数最多的平面方程所对应的点构成集合，利用该集合最小化距离函数，求得最优地面平面方程：

其中Dk表示集合中第k个点 到地面平面的距离.

（4）对剩余点云法向量计算差分特征（DoN），二次提取地面点云.

对于点云中的每一点q，在不同邻域r1，r2（r1>r2）进行法向量估计，得到两个单位法向量n(q,r1),n(q,r2)，向量间的夹角设为θ，定义法向量差分算子如下：

（5）对地面点云平面进行合并.

2.2 点云投影

剔除地面点云后的障碍物点云为Mi()xi,yi,zi，要实现对激光数据的模拟，需先将障碍物三维点云对地面进行投影，得到二维点云mi()θi,di，然后将Kinect v1每条视线上距离光心O最近的点提取出来，便形成了一束模拟激光，如图7所示.具体步骤如下：

图7 Kinect v1模拟激光原理图

（1）障碍物三维点云Mi()xi,yi,zi对地面进行投影得到二维点云mi()θi,di，其中：

（2）若Kinect相机观测角范围为()βmin,βmax，激光束共细分为N份.假设m()θ,d点为OC视线上距离光心O最近的点，那么，点m()θ,d在数组laser中的索引值为：

（3）数组laser［n］表示模拟激光数据，若某视线上产生模拟激光序号是n，该视线上激光点距离值为d[θ]，那么形成的模拟激光为：

3 视觉激光数据融合

单一传感器无法提供完整的环境测量数据，需要用到传感器融合技术.使用多个传感器提供冗余信息，可以减少测量误差.从相机和激光传感器获取传感器数据，通过传感器融合技术，可以将两个传感器用特定的方式结合起来，实现优势互补.本文首先对两组传感器数据进行点云配准实现粗融合，然后通过扩展卡尔曼滤波将两组激光数据进行精确融合.

3.1 点云配准

移动机器人系统上多传感器拥有多个坐标系，算法需要将多个传感器的数据统一到同一个基准坐标系进行对齐.对已获取的两束激光数据，通过对激光雷达的点云数据和深度相机的图像点云数据进行配准，实现数据的坐标系对齐，达到数据粗融合的目的.

图8所示为同一时刻分别处于激光雷达坐标系(OL,XL,YL,ZL)与Kinect v1相机坐标系(OK,XK,YK,ZK)中的点M，其坐标分别为(xL,yL,zL)和(xK,yK,zK)，根据激光雷达坐标系与相机坐标系的位姿关系，得到坐标转换关系为：

图8 坐标系位置关系示意图

式中，R是旋转矩阵；T是平移矩阵.

如图9（a）可知，激光点是对实际环境中曲面的离散采样，激光点到实际曲面的距离是最优的误差尺度.常用的点云配准算法经典迭代最邻近（Iterative Closest Point，ICP）算法［8］，如图9（b）所示，将点到点的距离，作为误差构建误差方程，容易造成误匹配，增加迭代时间.本文采用改进的PL-ICP（Point-to-Line）算法［9］，通过用点到其最近两点连线的距离代替点到点的距离，如图9（c）所示，构建误差方程，有利于缩减迭代时间，增加算法时效性.

图9 PL-ICP原理图

深度相机模拟的点云和对应的激光雷达点云空间坐标集合分别为：

算法的具体步骤为：

（1）给定一个初始的转换矩阵Qk=()Rk,Tk，将当前模拟点云的数据转换到激光雷达坐标系下，转换方式为：

后面迭代计算所需的数据由上一次算法迭代计算得到.

（3）计算误差并去除误差过大的点.

（4）构建最小化误差方程：

其中ni表示线的法向量.

（5）求解出位姿转换矩阵Qk+1，将其用于下次迭代计算.

3.2 传感器数据融合

在点云配准得到两个传感器之间的位姿关系后，可将两束激光转换到同一坐标系下进行数据精确融合.该过程常用滤波思想的方法实现，针对经典传感器滤波算法Kalman滤波［10］只局限用于系统线性、噪声为高斯理想场景的不足，本文采用基于其演变的扩展卡尔曼滤波［11］（EKF），在粗融合的基础上进行精细融合.

系统的状态转移方程以及传感器的观测方程分别为：

θk为估计值，zk为传感器的观测值，状态转移噪声向量sk一般是服从多维高斯分布的，其协方差矩阵为Q；观测噪声向量vk一般也是服从多维高斯分布的，其协方差矩阵是R.

利用泰勒展开式对（13）式在一次估计值θk-1处展开得：

利用泰勒展开式对（14）式在一次估计值θ′k处展开得：

其中，Fk-1和Hk分别表示f(θk-1)和h(θk)在θk-1和处的雅克比矩阵.

EKF分为预测和更新两步：

预测：

其中为预测值，为预测值与真实值的协方差.

更新：

其中Kk为卡尔曼增益，θk为估计值，zk为观测值，Pk为估计值与真实值之间的协方差.

如图11所示，具体融合过程为：

图10 融合算法示意图

（1）在t时收到激光雷达数据时，根据t-1时刻的估计值θt-1和协方差Pt-1，利用公式（17）、（18）完成一次预测得到预测值和协方差.

（2）再根据t时刻的激光雷达的观测数据、预测值和协方差，利用公式（19）、（20）、（21）实现测量值更新得到t时刻的状态θt和协方差Pt.

（3）在t+1时刻收到Kinect v1模拟激光数据时，根据t时刻的状态θt和协方差Pt完成一次预测得到预测值和协方差

（4）同理，根据t+1时刻的Kinect v1模拟激光的观测数据、预测值和协方差，实现测量值更新得到t+1时刻的估计值θt+1和协方差Pt+1用于下一次迭代.

4 实验与分析

实验平台如图11所示基于Aimibot教育机器人，硬件包括NUC机载电脑、高清显示器、激光雷达LS01B、RGB-D相机Kinect v1等，其中机载电脑搭载基于Linux（Ubuntu16.04）系统的ROS（Robot Operating System）操作系统.

图11 实验平台

综合考虑实验室的条件以及实验平台的局限性，为验证视觉激光融合建图算法在复杂环境下的建图准确性与可靠性，设置如图12所示的实验场景.场景面积为8 m×5 m，主要包括三种不同障碍物，分别为组合体A、镂空的凳子B、小木块C.

图12 实验场景

场景中长方体木块的宽为10 cm，而实验平台的单线激光雷达离地的高度为15 cm，因而场景中的一部分障碍物不在单线激光雷达的扫描平面.三种障碍物依次排开，与机器人形成的夹角为60°，而Kinect v1深度相机的视角为57°×43°，因此机器人无法感知到所有障碍物，如图13所示.

图13 机器人与障碍物相关位置关系

实验主要是对比采用不同传感器数据建图的结果，以验证本文算法的可行性和有效性.

如图14为单个二维激光雷达的建图结果，该结果表明二维激光雷达无法感知非激光扫描平面障碍物信息，对场景中障碍物的感知情况为：组合体A中比激光雷达位置低的部分和镂空凳子B与激光雷达不在同一平面的部分均出现信息缺失.此地图在机器人导航时会使其产生错误的路径规划.

图14 二维激光数据建图结果

文献［1］采用的视觉激光融合算法，在进行RGB-D相机模拟激光数据时，处理地面点云时仅对深度图像固定高度范围内像素进行转换，缩小了RGB-D相机的感知范围，不能根据机器人所处实际环境自适应剔除地面点云.因而机器人在移动建图过程中对同一障碍物不能连续观测，导致部分障碍物信息缺失，如图15所示障碍物均出现缺损.

图15 文献［1］融合方法建图结果

文献［2］采用的传统ICP算法进行点云配准，算法精度和时效性较低，导致两传感器之间的位姿关系不能实时准确更新，使得融合过程数据参考坐标系不统一，最终融合结果出现重影，如图16所示，建图精度降低，无法准确还原障碍物的信息.

图17 本文融合方法建图结果

图7为本文视觉激光融合数据建图结果，对比单个二维激光雷达数据和文献［1］［2］融合方法数据建图结果，很好地还原了场景的障碍物信息，建图的完整度大大提高，包含了更为丰富的环境信息，同时也保证了建图的精度.

图16 文献[2]融合方法建图结果

通过该实验的对比可以看出，融合多传感器数据能够有效弥补单一传感器的不足，实现对环境的更精确、更完整的感知.利用此融合算法可以实现机器人在复杂环境下的精确定位和导航.

5 结语

本文分析了当前移动机器人环境感知传感器的优缺点，针对当前一些传感器融合方案的不足，提出一种改进RGB-D相机和激光雷达数据融合的算法.利用融合后的数据进行建图，并对比单个传感器数据和已有传感器融合方案的建图结果，本文的融合算法在保证了传感器数据精度的基础上，实现了对环境信息更全面的感知.这对提高移动机器人建图完整度和精准度有重要意义.