CN105783913A - 一种融合车载多传感器的slam装置及其控制方法 - Google Patents

Abstract

本发明涉及融合车载多传感器的三轮全向移动机器人的同步定位与建图方法的技术领域，更具体地，涉及一种融合车载多传感器的SLAM装置及其控制方法。一种融合车载多传感器的SLAM装置，其中，包括车载传感器、车载编码器、车载惯性测量单元、车载控制器、上位机；所述的车载编码器和车载惯性测量单元连接车载控制器，所述的车载控制器连接上位机；所述的车载传感器与上位机连接。能够提高机器人定位和建图的效果，解决单纯依靠RGB‑D传感器进行SLAM的深度值缺失或特征点稀少带来的定位和建图的误差问题，从而提高SLAM的鲁棒性和准确性。

Description

一种融合车载多传感器的SLAM装置及其控制方法

技术领域

本发明涉及融合车载多传感器的三轮全向移动机器人的同步定位与建图方法的技术领域，更具体地，涉及一种融合车载多传感器的SLAM装置及其控制方法。

背景技术

全向移动机器人能实现任意方向的运动，可广泛应用于军事、工业、家用服务等领域。移动机器人的同时定位和地图创建(SLAM，Simultaneous LocalizationAnd Mapping)是机器人领域的热点研究问题，它是移动机器人自助任务规划和路径规划的前提和基础。机器人的SLAM问题，简单的说就是在一个未知的环境中，移动机器人需要建立环境地图，并在地图上的同时定位自身。这个过程类似于人走到一个完全陌生的环境中，在未携带任何能够确定位置和方向的设备情况下，只能根据对周围环境的观察和对自身运动的估计认识环境并确定自己的位置。

SLAM本质上是一个系统状态(包括机器人当前位姿以及所有地图特征位置等)估计问题。从这一角度，其求解方法可大致分为基于Kalman滤波器的方法、基于粒子滤波器的方法、基于图优化的方法3类。基于Kalman滤波(KF)和粒子滤波(PF)的方法主要依据递归贝叶斯状态估计理论。而基于图优化的增量式SLAM算法框架，主要包括顺序数据关联、环形闭合检测以及图优化3大部分。

从传感器的角度分析，在室外环境中，定位的问题通常可以由GPS(GlobalPositioning System)来完成，因为GPS为机器人所承担的任务提供了很好的准确性。然而，当在室内移动时，GPS数据不可用，难以准确估计机器人的位置，因此必须寻找其它解决方案。以前大部分的机器人地图构建只能提供环境的二维信息，因为获得高质量的三维数据非常昂贵或者对于机器人的运动有很多的约束。这一阶段的研究使用的数据源一般为激光测距仪，双目摄像头和单目摄像头。激光测距仪的定位精度虽然高，但价格昂贵，三维激光扫描仪的价格更是让人难以承担。而基于双目或单目的SLAM依赖于计算机视觉和图像处理技术，缺点在于缺乏直观的场景深度信息，三维地图重建工作变得复杂。最近几年，随着微软公司的Kinect、华硕公司的Xtion Pro Live等便宜且高效的RGB-D摄像机的出现，采用RGB-D传感器进行移动机器人定位或SLAM的研究迅速发展起来。

RGB-D传感器能够同时提供彩色(RGB)和深度(Depth)图像信息，相比于激光测距仪，它能够更廉价地获得深度信息，而相比于普通相机，它连续性好，环境信息更加丰富，因此定位的精度更高，且更容易进行三维地图重建。而采用RGB-D摄像机对环境进行SLAM的最新研究中，则有相当一部分都采用了基于图优化的SLAM方法。

但RGB-D传感器也存在可获得的深度值有效范围较小的问题，对于一些空旷且较大的场景，会出现深度值缺失或特征点缺失的问题，影响运动估计的准确性，甚至导致定位和建图出现严重偏差。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供一种融合车载多传感器的SLAM装置及其控制方法，基于图优化的SLAM方法，考虑RGB-D传感器的优点并针对其局限性，引入车载的编码器和惯性测量单元两类传感器，提出融合车载多传感器的SLAM方法，通过融合多传感器的数据，计算得到全向移动机器人的位姿变换，减小由于深度值缺失以及特征点稀少所造成的误差，使获得的机器人运动轨迹更为平滑准确，同时也提高建图的准确度。

为解决上述技术问题，本发明采用的技术方案是：一种融合车载多传感器的SLAM装置，其中，包括车载传感器、车载编码器、车载惯性测量单元、车载控制器、上位机；所述的车载编码器和车载惯性测量单元连接车载控制器，所述的车载控制器连接上位机；所述的车载传感器与上位机连接。所述的车载传感器为Kinect体感传感器。

本发明中，在未知环境下，三轮全向移动机器人在运动的过程中通过车载RGB-D传感器(即Kinect体感传感器)获取周围环境的彩色图像和深度图像，从图像变化解算出自身的运动变换，同时还通过车载编码器和车载惯性测量单元获得机器人自身的运动数据，对这些数据进行融合处理，以获得机器人自身所处的位置并构建出周围环境的三维地图，实现机器人的同步定位与建图(SLAM)。

利用所述的融合车载多传感器的SLAM装置的控制方法，其中，包括以下步骤：

S1.车载传感器将获取的图像信息传送到上位机，解算出机器人位移和姿态信息；

S2.车载编码器和车载惯性测量单元收集机器人的位移和旋转信息发送到车载控制器，再通过车载控制器传送到上位机上，解算出机器人位移和姿态变化；

S3.将车载传感器、车载编码器、车载惯性测量单元的信息融合，对机器人进行同步定位和建图。

本发明中，在行进过程中，移动机器人通过RGB-D传感器(即Kinect体感传感器)获取周围环境的图像，由上位机采集编码器和惯性测量单元数据，分别解算出机器人的位姿，然后进行数据融合，以获得更准确的位姿信息，从而提高机器人定位和建图的准确度。车载编码器和惯性测量单元收集机器人的位移和旋转信息发送到车载控制器，再通过车载控制器传送到上位机上；车载Kinect直接与上位机连接，将获取的图像信息传送到上位机，解算出机器人位姿信息后，与编码器和惯性测量单元的信息进行融合。

具体的，所述的步骤S1中，包括以下步骤：

S11.特征提取与匹配；

S12.帧间变换估计。

所述的步骤S11中，包括以下步骤：特征点检测、描述符提取、特征点匹配。

所述的步骤S3中，包括以下步骤：

S31.加权平均的方法对车载传感器、车载编码器、车载惯性测量单元的数据进行融合，优化位姿估计；

S32.关键帧检测与闭环检测；

S33.位姿图优化；

S34.拼接点云建图。

与现有技术相比，有益效果是：本发明在未知的环境中，全向移动机器人采用车载传感器获取周围环境的信息，进行同步定位和建图，我们所考虑的车载传感器系统包括RGB-D传感器、编码器和惯性测量单元。由于Kinect存在深度范围有限的问题，且基于图像匹配估计运动的方式容易受到特征点缺失的影响，导致估计结果误差较大，因此我们考虑融合Kinect、编码器和惯性测量单元的数据，优化位姿估计。基于所提出的多传感器加权融合算法，本方法能够提高机器人定位和建图的效果，解决单纯依靠RGB-D传感器进行SLAM的深度值缺失或特征点稀少带来的定位和建图的误差问题，从而提高SLAM的鲁棒性和准确性。

融合车载编码器和惯性测量单元所估计的数据，较好地解决了RGB-DSLAM中存在的深度范围有限、深度值缺失导致的定位和建图的偏差，提高SLAM方法的鲁棒性；

采用加权平均的方式融合多传感器解算得到的位姿信息，提高了位姿的准确性，从而提高SLAM的效果和准确性；

把多传感器融合估计位姿的方法应用于SLAM系统，并提供一个较为完整的SLAM解决方案。

附图说明

图1是融合多传感器的SLAM系统结构图。

图2是三轮全向机器人机械结构示意图。

图3是世界坐标系图。

图4是机器人坐标系图。

图5是合速度分解图。

图6是多传感器数据融合方法流程框图。

图7是融合多传感器的SLAM方法流程框图。

图8是机器人x轴上的轨迹跟踪效果对比图。

图9是机器人y轴上的轨迹跟踪效果对比图。

图10是只使用Kinect的建图效果图。

图11是使用多传感器加权融合方法的建图效果图。

图12是机器人x轴上的轨迹跟踪效果对比图。

图13是机器人y轴上的轨迹跟踪效果对比图。

图14是只使用Kinect的建图效果图。

图15是使用多传感器加权融合方法的建图效果图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

如图1所示，一种融合车载多传感器的SLAM装置，其中，包括车载传感器、车载编码器、车载惯性测量单元、车载控制器、上位机；所述的车载编码器和车载惯性测量单元连接车载控制器，所述的车载控制器连接上位机；所述的车载传感器与上位机连接。所述的车载传感器为Kinect体感传感器。

本发明中，在未知环境下，三轮全向移动机器人在运动的过程中通过车载RGB-D传感器(即Kinect体感传感器)获取周围环境的彩色图像和深度图像，从图像变化解算出自身的运动变换，同时还通过车载编码器和车载惯性测量单元获得机器人自身的运动数据，对这些数据进行融合处理，以获得机器人自身所处的位置并构建出周围环境的三维地图，实现机器人的同步定位与建图(SLAM)。

(一)基于RGB-D传感器的位姿估计

RGB-D传感器采用微软公司的Kinect体感传感器。Kinect有一个彩色摄像头，用来获取640*480的彩色图像，每秒最多获取30帧图像；彩色摄像头两边分别是红外发射器和红外接收器,，它们共同组成景深摄像头，用以检测物体距离Kinect的深度信息。

1、特征提取与匹配

常用的图像特征提取算法有SIFT(Scale-invariant feature transform)，SURF(Speeded Up Robust Features)等算法。SIFT算法基于尺度空间，能够不受图像的缩放、转动以及仿射变换的影响。SURF算法是由Herbert Bay提出的一种基于二维Hessian矩阵的算法，该算法是在SIFT算法上做的改进，提高了算法的执行效率。本方法采用SURF算法对Kinect获取的彩色图像进行特征点检测和描述符提取，采用FLANN的特征匹配方法进行特征匹配。

(1)特征点检测：首先对原图像构建尺度空间金字塔，然后用Hessian矩阵求出极值，在3×3×3的立体邻域内进行非极大值抑制，找出候选特征点，再在尺度空间和图像空间中进行插值运算，得到稳定的特征点位置及所在的尺度值，最后利用像素的Haar小波响应，通过统计60°范围内像素点的水平和垂直Haar小波响应来指定特征点的方向；

(2)描述符提取：以特征点为中心，首先将坐标轴旋转到主方向，按照主方向选取边长为20s的正方形区域，将该窗口区域划分成4×4的子区域，在每一个子区域内，计算5s×5s(采样步长取s)范围内的小波响应，相对于主方向的水平、垂直方向的Haar小波响应分别记做d_x、d_y，同样赋予响应值以权值系数，以增加对几何变换的鲁棒性；然后将每个子区域的响应以及响应的绝对值相加形成∑d_x，∑d_y，∑|d_x|，∑|d_y|。这样，在每个子区域形成四维分量的矢量V_sub＝(∑d_x,∑|d_x|,∑d_y,∑|d_y|)，因此，对每一特征点，形成4×(4×4)＝64维的描述向量，再进行向量的归一化，从而对光照具有一定的鲁棒性。

(3)特征点匹配：使用FLANN(Fast Library for Approximate NearestNeighbors)进行特征点匹配。FLANN是在高维空间执行快速近似最近邻搜索的一个库，它包含一组优秀的最近邻搜索算法和一个自动选择最优算法并且根据数据集对算法参数进行优化的系统；对于高维空间中的最近邻搜索问题，FLANN采用基于分层k-均值树的优先级搜索算法或采用多重随机k-d树时具有最好的性能；相对于其他传统最近邻搜索算法，基于FLANN的近似快速最近邻搜索的速度提高了一个数量级。

2、帧间变换估计

得到两帧彩色图像之间的一组匹配特征点后，就可结合对应的深度图像的信息计算两个图像间的转换关系。设两个帧分别为F₁和F₂，则一组一一对应的匹配特征点为：

P＝{p₁,p₂,...,p_N}∈F₁, (1)

Q＝{q₁,q₂,...,q_N}∈F₂, (2)

其中p_i和q_i都是R²中的点，i＝1,…,N。

三维空间点[x,y,z]和它在图像中的像素坐标[u,v,d](d指深度数据)的对应关系为：

$\left\{\begin{array}{c}u=\frac{x\·f_x}{z}+c_x,\\ u=\frac{y\·f_y}{z}+c_y,\\ d=z\·s,\end{array}\right.---\left(3\right)$

其中，f_x、f_y指相机在两个轴上的焦距，c_x、c_y指相机的光圈中心，s指深度图的缩放因子。利用此公式，将帧F₂中的特征点映射到当前帧的相机坐标系中，可得：

Q_c＝{q_c1,q_c2,...,q_cN}, (4)

其中，q_ci是R³中的点，i＝1,…,N。

则变换矩阵T求解如下：

$T^{*}=\underset{T}{\mathrm{argmin}}\underset{i=1,...,N}{\Σ}\left|\right|f(q_{ci};T)-p_i|{|}^2,---\left(5\right)$

其中，f是投影矩阵。通过最小化重投影误差的方法，可以获得比直接采用三维点对进行计算并最小化空间距离更好的效果。

RANSAC(Random Sample Consensus)是一种迭代算法，用于去除图像特征点中的一些离群点，提高匹配精度。RANSAC算法是一种比最小二乘法更优的求取模型参数的算法，对于一组包含有噪声点的数据集，除了可以找到最佳参数，还能够剔除这些噪声点。本方法结合RANSAC算法剔除错误匹配，以提高变换矩阵的准确性。通过使用RANSAC算法多次迭代求解，从而获得更准确的变换估计。

(二)基于编码器和惯性测量单元的位姿估计

本方法考虑的三轮全向移动机器人采用了正三角的三轮结构，长85cm，宽78cm，高50cm，采取全向轮，可实现任意方向的移动。机器人上搭载了编码器、惯性测量单元，可解算出机器人的位移和姿态变化。

本方法采用双向光电编码器，转动轴与电机通过联轴器连接，能够在电机转动时产生三路方波，称作A相、B相和Z相。

三轮全向移动机械结构如图2，其中L₁＝L₂＝L₃＝L(L表示驱动轮中心到机器人中心的距离)，三个驱动轮的旋转轴互成120°，V₁、V₂、V₃表示驱动轮的运动方向。

三轮全向机器人的工作环境为一平面空间，建立世界坐标系X–Y，如图3所示；以全向轮小车的中心点为原点，L₂所在直线方向为y轴，建立机器人的体坐标系x_a-y_a，如图4所示；在世界坐标系中对机器人的整体速度进行x轴和y轴方向上的分解，如图5。

图3中的θ_c为世界坐标X轴与机器人坐标x_a轴的夹角，假设机器人在零状态时其夹角为零，即世界坐标轴与机器人起始点的坐标轴重合，则θ_c表示机器人运动时产生的自旋角度。图5中的β表示合速度V与世界坐标的X轴的夹角，V分解为V_x和V_y。 $V=\[\begin{array}{ccc}{V}_x& V_y& {\stackrel{\·}{\mathrm{\θ}}}_c\end{array}\]$ 表示小车的姿态变量。三轮全向移动机器人的运动学模型为:

$\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\end{array}\right]=\left[\begin{array}{ccc}-sin(\frac{\mathrm{\π}}{6}-{\mathrm{\θ}}_c)& -cos(\frac{\mathrm{\π}}{6}-{\mathrm{\θ}}_c)& L\\ {\mathrm{cos\θ}}_c& {\mathrm{sin\θ}}_c& L\\ -sin(\frac{\mathrm{\π}}{6}+{\mathrm{\θ}}_c)& -sin(\frac{\mathrm{\π}}{6}+{\mathrm{\θ}}_c)& L\end{array}\right]\left[\begin{array}{c}{V}_x\\ V_y\\ {\stackrel{\·}{\mathrm{\θ}}}_c\end{array}\right]---\left(6\right)$

将编码器输出的A相和B相输入到测速电路模块中，可以得到机器人三个轮子的速度，即V₁、V₂、V₃。根据等式(6)，可计算出机器人的水平速度V_x和纵向速度V_y，积分即可获得三轮全向机器人相对于起始点位置的相对位移x和y。

惯性测量单元包括加速度计和陀螺仪，加速度计用于测量在机器人坐标下的三个坐标轴方向上的加速度，陀螺仪用于测量机器人体坐标下的三个坐标轴方向上的角速度，通过积分可以得到机器人旋转角度。通过数据融合算法融合惯性测量单元中的加速度与角速度信息，可以估计得到ψ、θ、三个姿态角度，分别对应机器人绕Z轴、Y轴和X轴的旋转角度，即机器人的位姿信息。

(三)融合多传感器的机器人同步定位和建图方法

本方法对Kinect、编码器和惯性测量单元的数据进行融合估计位姿，使用了关键帧减少冗余图像帧以提高SLAM效率，并对关键帧进行闭环检测以增加帧间关联，同时利用帧间关联进行位姿图优化以减小累积误差，通过以上方法提高定位和建图的准确度。

1、加权平均的多传感器数据融合方法

本方法采用加权平均的方法对Kinect、编码器和惯性测量单元的数据进行融合，优化位姿估计。

用p表示用RANSAC算法求解帧间变换时的局内点对数目和用于求解变换的点对总数目的比值，0≤p≤1；则当p越大，该算法求解的变换矩阵越可信，p的比值由求解变换的每两帧图像确定。此外，用q作为衡量编码器数据准确性的因子，用r作为衡量惯性测量单元数据准确性的因子，q和r的值越大则对应的传感器数据的准确度越高。当采用不同精确度的传感器时，所对应的描述准确度的因子亦不同。在对多传感器数据进行融合时，可以通过实验的方法选择适宜的q和r的值。

利用Kinect获取的图像信息可解算出帧间的平移向量和旋转向量，由车载编码器可算出帧间的平移向量，而由车载惯性测量单元可估计得到帧间的旋转向量。

本方法采用如下规则对多传感器的数据进行融合：

(1)获取位移变换时，将由Kinect获取的图像信息解算获得的平移向量与由编码器的测量信息估计得到的帧间位移差值以的比值进行融合，从而获得当前帧与前一帧机器人的位移变换。机器人在二维平面运动，只有X和Y方向的位移变化，因此位移变换融合公式如下：

$\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\end{array}\right]=\frac{p}{p+q}\left[\begin{array}{c}\mathrm{\Δ}{x}_k\\ {\mathrm{\Δy}}_k\end{array}\right]+\frac{q}{p+q}\left[\begin{array}{c}\mathrm{\Δ}{x}_c\\ \mathrm{\Δ}yc\end{array}\right],---\left(7\right)$

其中，Δx_k和Δy_k分别表示Kinect解得的机器人沿x方向和沿y方向的帧间位移，Δx_c和Δy_c表示编码器解得的帧间位移，Δx和Δy表示加权融合后的帧间位移。

(2)获取姿态变换时，将由Kinect获取的图像信息解算获得的旋转向量与由惯性测量单元的测量信息估计得到的帧间姿态差值以的比值进行融合，从而获得当前帧与前一关键帧机器人的姿态变换。机器人在二维平面运动，只有绕Z轴旋转的角度ψ的变化，因此姿态变换融合公式如下：

$\mathrm{\Δ}\mathrm{\ψ}=\frac{p}{p+r}{\mathrm{\Δ\ψ}}_k+\frac{r}{p+r}{\mathrm{\Δ\ψ}}_m,---\left(8\right)$

其中，Δψ_k表示Kinect解得的机器人绕Z轴的帧间姿态角变化，Δψ_m表示惯性测量单元解得的帧间姿态角变化，Δψ表示加权融合后的帧间姿态角变化。

(3)因为用于求解最小化重投影误差的点对至少需要4组，当利用Kinect获取的图像信息求解的局内点数小于5组时，解算出的变换矩阵误差很大。在这种情况下舍弃由Kinect获取的图像数据的估计值，而使用由编码器和惯性测量单元所获得的解算结果。

多传感器数据加权融合方法流程图如图6所示。

2、关键帧检测与闭环检测

Kinect每秒钟可以获取30帧的图像，因此帧与帧之间距离很近，如果把每一帧都用于构建地图，会导致地图频繁更新，消耗计算时间与储存空间。因此我们采取了提取关键帧的方法，剔除冗余帧以提高效率。算出两帧之间的运动变换矩阵T^*的二范数e，若e小于设定的阈值E_min，则两帧之间变换太小，为冗余帧，丢弃当前帧；若e大于阈值E_min，则取为关键帧。E_min太小会导致剔除的冗余帧过少，从而降低效率，E_min过大则相邻关键帧的图像相差较多，运动估计容易出错。E_min的选取可根据不同的数据集进行测试，选取适宜的值。本方法选取的E_min为0.1。

对提取的当前关键帧进行闭环检测，用关键帧分别与前N个关键帧和整个关键帧序列中的随机M帧进行帧间变换估计，算出两帧之间的运动变换矩阵T^*。类似于关键帧检测，计算T^*的二范数e，当e大于E_min且小于E_max时，判定这两帧是机器人在相近的位置取得，即机器人又回到曾到过的位置，则将算得的变换矩阵T^*作为新的约束关系加入位姿图中，以便进行位姿图优化时能更好地消减误差。E_max太小，则闭环检测成功率低，E_max太大，则容易出现错误的闭环匹配，导致位姿图优化时误差更大。E_max的选取也可根据不同的数据集进行测试，选取适宜的值。本方法选取的E_max为1.0。

3、位姿图优化

经过位姿融合、关键帧提取、闭环检测等步骤后，会得到一系列的位姿点之间的变换关系。因此，对位姿进行全局一致优化的问题可用图来表示，也就是目前视觉SLAM里常用的姿态图(pose graph)。图中的每个节点表示一个状态变量，即相机的各个姿态v_i：

v_i＝[x,y,z,q_x,q_y,q_z,q_w]_i, (9)

其中，x、y、z表示相机的位置，q_x、q_y、q_z、q_w用四元数表示相机的姿态角。

每条边E_i,j表示连接两个相邻状态变量v_i和v_j之间的测量值，即两帧之间的变换矩阵T_i,j：

$E_{i,j}=T_{i,j}={\left[\begin{array}{cc}{R}_{3*3}& t_{3*1}\\ O_{1*3}& 1\end{array}\right]}_{i,j},---\left(10\right)$

其中，R_3*3表示旋转矩阵，可由旋转向量通过罗格里德斯变换获得，t_3*1表示平移向量。

四元数的表示形式可与旋转矩阵的形式相互转换，所以节点v_i的状态变量也可用变换矩阵的形式x_i表示：

$x_i={\left[\begin{array}{cc}{R}_{3*3}& t_{3*1}\\ O_{1*3}& 1\end{array}\right]}_i---\left(11\right)$

理想的情况下，应有如下变换关系：

x_i＝T_i,jx_j (12)

但由于边中存在误差，使得所有的边给出的数据并不一致，因此转化成求解误差函数的最小值的问题：

$\min E=\underset{i,j}{\Σ}\left|\right|x_i^{*}-T_{i,j}{x}_j^{*}|{|}_2^2,---\left(13\right)$

其中，表示优化变量x_i的估计值。

本方法采用g2o(General Graph Optimization)算法，即通用图优化算法，来进行位姿图的优化，求解出最优位姿x*的组合，让误差E收敛到一个极小值。g2o是一个算法集，根据具体的问题选用合适的算法求解优化问题。

优化后的位姿图包含了各个关键帧对应的相机位置和姿态，可据此绘出机器人的运动轨迹。

4、拼接点云建图

利用RGB-D传感器获取的彩色信息和深度信息将每帧图像转化为点云。利用公式(3)可以将每个图像点的二维坐标映射到当前帧相机坐标系的三维坐标，再加入彩色RGB信息，就可获得点云p_c＝[x y z r g b]。

点云的拼接，实质上是对点云做变换的过程。这个变换往往是用变换矩阵来描述的，即公式(10)，变换矩阵T结合了旋转和缩放，可对点进行齐次变换：

$\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\\ 1\end{array}\right]=T\·\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ 1\end{array}\right]---\left(14\right)$

本方法采用了PCL库里提供的点云变换函数，给定变换矩阵，就可对整个点云进行移动，完成点云的拼接。

本方法提供了一个较为完整的SLAM解决方案，包括了特征的提取与匹配、运动估计、关键帧检测、闭环检测、地图优化、建图与定位等步骤，并将多传感器加权融合的方法应用于位姿估计，提高了SLAM的准确度。本方法提供的SLAM方案的具体流程如图7所示。

经过多次实验测量，本方法使用的编码器因子q设置为0.2，惯性测量单元因子r设置为0.3。

图8至11为针对同一数据集进行的SLAM方法的比较，包括只使用Kinect数据以及融合了多传感器数据两种方法的即时建图和定位对比图。

图8、9中方框中上部线条表示只使用车载Kinect获取的图像进行机器人自身运动估计时算出的机器人的运动轨迹，方框中下部线条表示融合车载Kinect、编码器和惯性测量单元获取的数据进行运动估计时算出的运动轨迹。当局内点对数目和用于RANSAC求解的点对总数目的比值p较大时，Kinect的解算结果可信度较高，融合多传感器方法获得的位置与Kinect解算出的位置相对较近；当p较小时，Kinect的解算结果可信度较低，融合多传感器方法获得的位置与Kinect解算出的位置相对较远。

可以看出，采取了加权融合的多传感器融合方法后，只使用Kinect时出现的明显的运动估计错误(即尖刺)得到了修正，提高了机器人定位的效果和鲁棒性。

图10为只使用Kinect的图像信息时的建图效果，图11为使用多传感器加权融合方法的建图效果。由图中可以看到，图10的圈内是因运动估计误差较大而导致的建图偏差；而在图11中，由于使用了多传感器加权融合方法，这个问题得到了修正。

图12至15为使用另一数据集进行的SLAM方法的比较，包括只使用Kinect数据以及融合了多传感器数据两种方法的即时建图和定位对比图。

类似的，图12、13中方框中上部线条表示只使用车载Kinect获取的图像信息进行机器人自身运动估计时算出的机器人的运动轨迹，方框中下部线条表示融合车载Kinect、编码器和惯性测量单元获取的数据进行运动估计时算出的运动轨迹。

在图中可以看到，只使用Kinect的图像信息时，机器人的运动轨迹跟踪到中间处丢失了，这是由于Kinect的深度测量范围有限，只能探测到4米内的物体，当机器人运动到较空旷的位置时，Kinect无法获得足够的深度值去解算帧间的运动变换，只能丢弃当前帧；在连续丢弃多帧后，即使机器人再次回到深度值丰富的地方，获得的当前帧与丢帧之前的最后一个关键帧也因相差太远而难以匹配，最终导致机器人的定位失败；而采用多传感器加权融合方法后，机器人在深度值缺失的情况下可以采用编码器和惯性测量单元继续估计自身运动，很好地弥补了Kinect深度测量范围有限的问题。

图14为只使用Kinect的图像信息时的建图效果，图15为使用多传感器加权融合方法的建图效果。由图中可以看到，图14的圈内是因为机器人定位失败而缺失的部分，而图15中这个缺失的部分得到弥补，改善了建图的效果。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (7)

1.一种融合车载多传感器的SLAM装置，其特征在于，包括车载传感器、车载编码器、车载惯性测量单元、车载控制器、上位机；所述的车载编码器和车载惯性测量单元连接车载控制器，所述的车载控制器连接上位机；所述的车载传感器与上位机连接。

2.根据权利要求1所述的一种融合车载多传感器的SLAM装置，其特征在于：所述的车载传感器为Kinect体感传感器。

3.利用权利要求2所述的融合车载多传感器的SLAM装置的控制方法，其特征在于，包括以下步骤：

S1.车载传感器将获取的图像信息传送到上位机，解算出机器人位移和姿态信息；

S2.车载编码器和车载惯性测量单元收集机器人的位移和旋转信息发送到车载控制器，再通过车载控制器传送到上位机上，解算出机器人位移和姿态变化；

S3.将车载传感器、车载编码器、车载惯性测量单元的信息融合，对机器人进行同步定位和建图。

4.根据权利要求3所述的融合车载多传感器的SLAM装置的控制方法，其特征在于：所述的步骤S1中，包括以下步骤：

S11.特征提取与匹配；

S12.帧间变换估计。

5.根据权利要求4所述的融合车载多传感器的SLAM装置的控制方法，其特征在于：所述的步骤S11中，包括以下步骤：特征点检测、描述符提取、特征点匹配。

6.根据权利要求3所述的融合车载多传感器的SLAM装置的控制方法，其特征在于：所述的步骤S3中，包括以下步骤：

S31.加权平均的方法对车载传感器、车载编码器、车载惯性测量单元的数据进行融合，优化位姿估计；

S32.关键帧检测与闭环检测；

S33.位姿图优化；

S34.拼接点云建图。

7.根据权利要求6所述的融合车载多传感器的SLAM装置的控制方法，其特征在于：所述的步骤S31中，

用p表示用RANSAC算法求解帧间变换时的局内点对数目和用于求解变换的点对总数目的比值，0≤p≤1；用q作为衡量编码器数据准确性的因子，用r作为衡量惯性测量单元数据准确性的因子；

获取位移变换时，将由Kinect获取的图像信息解算获得的平移向量与由编码器的测量信息估计得到的帧间位移差值以的比值进行融合，从而获得当前帧与前一帧机器人的位移变换；机器人在二维平面运动，只有X和Y方向的位移变化，因此位移变换融合公式如下：

$\left[\begin{array}{c}\mathrm{\Δ}x\\ \mathrm{\Δ}y\end{array}\right]=\frac{p}{p+q}\left[\begin{array}{c}\mathrm{\Δ}{x}_k\\ {\mathrm{\Δy}}_k\end{array}\right]+\frac{q}{p+q}\left[\begin{array}{c}\mathrm{\Δ}{x}_c\\ \mathrm{\Δ}yc\end{array}\right],---\left(1\right)$

其中，Δx_k和Δy_k分别表示Kinect解得的机器人沿x方向和沿y方向的帧间位移，Δx_c和Δy_c表示编码器解得的帧间位移，Δx和Δy表示加权融合后的帧间位移；

获取姿态变换时，将由Kinect获取的图像信息解算获得的旋转向量与由惯性测量单元的测量信息估计得到的帧间姿态差值以的比值进行融合，从而获得当前帧与前一关键帧机器人的姿态变换；机器人在二维平面运动，只有绕Z轴旋转的角度ψ的变化，因此姿态变换融合公式如下：

$\mathrm{\Δ}\mathrm{\ψ}=\frac{p}{p+r}{\mathrm{\Δ\ψ}}_k+\frac{r}{p+r}{\mathrm{\Δ\ψ}}_m,---\left(2\right)$

其中，Δψ_k表示Kinect解得的机器人绕Z轴的帧间姿态角变化，Δψ_m表示惯性测量单元解得的帧间姿态角变化，Δψ表示加权融合后的帧间姿态角变化；

用于求解最小化重投影误差的点对至少需要4组，当利用Kinect获取的图像信息求解的局内点数小于5组时，解算出的变换矩阵误差很大；在这种情况下舍弃由Kinect获取的图像数据的估计值，而使用由编码器和惯性测量单元所获得的解算结果。