CN106153048A - 一种基于多传感器的机器人室内定位及制图系统 - Google Patents

Abstract

本发明公开了一种基于多传感器的机器人室内定位及制图系统，该系统包括激光定位、视觉闭环检测和地图优化模块，所述激光定位由激光帧间匹配和激光闭环检测两部分组成，所述激光帧间匹配获取激光传感器的信息，并将匹配结果输入视觉闭环检测；所述视觉闭环检测使用视觉信息进行闭环检测，并根据检测结果判断是否需要执行激光闭环检测，同时将检测结果输入地图优化模块；所述地图优化模块根据闭环检测结果进行姿态估计，并将估计结果用于反馈闭环检测。本方案中的视觉信息仅用于建立视觉特征，省去了建立视觉地图、更新视觉地图的步骤，在利用激光SLAM快速定位的同时，引入准确的视觉闭环检测。

Description

一种基于多传感器的机器人室内定位及制图系统

技术领域

本发明涉及机器人自主导航，具体涉及一种基于多传感器的机器人室内定位及制图系统。

背景技术

伴随着信号处理、人工智能、机械制造等技术的进步，拥有环境感知、自主导航以及人机交互的机器人的需求也越来越强烈。具有自主意识的机器人要求机器人对于环境有很好的感知、判断和适应能力，从而可以完成很多人类希望它们去完成的任务。机器人的自主性就是在未知的环境中通过机器人的自主决策来完成一些任务。而在未知的环境中，机器人的自主导航是研究中的一个难点。

对于家庭服务式机器人来说，大部分的运行环境都是在室内，而室内环境一般比较复杂，所以室内导航的好坏直接影响家庭服务机器人的各种功能。要完成地图的创建，首先要获取周围环境的信息，这就要用到传感器来完成。现在机器人的制图研究热点大体上集中在两种传感器上，一种是激光传感器，这种传感器有很好的快速性，并且可以得到很准确地距离等信息；另一种比较常用的传感器就是视觉传感器，包括单目、双目以及深度相机等。这两种常用的传感器各自有自己的优点以及缺点，适用于不同的环境。对于机器人来说，制图不仅要有一定的精度，并且对于快速性也有很高的要求，否则无法完成同步定位与制图。

在小型环境中，激光传感器拥有很高的制图精度，而到了大型环境中，机器人运行一段时间后，由于累积误差的存在地图会有较大程度的形变。现有的激光SLAM算法中，对于这个问题的解决，一般是引进闭环检测来消除累积误差。但是由于激光传感器所采集的信息量较少，所以在判断闭环时从实际闭环点到判断出来的闭环点之间有一个较大的误差。而且当机器人处在几何环境特征重复率很高的环境中时，单独根据激光传感器所得的信息来判断闭环有一定概率出现错误匹配。

发明内容

本发明目的在于克服现有技术的不足，尤其解决现有的基于激光传感器的机器人室内定位方法累积误差高、采集的信息量较少、匹配精度不高的问题。

本发明方案用单目视觉信息来辅助进行激光SLAM室内定位，利用单目视觉方法来检测闭环并给出一个闭环检测信号帮助解决激光闭环检测成功率过低的问题，提出使用对极几何约束过滤错误匹配的视觉局部特征，提高了视觉局部特征的匹配准确率，保证了视觉匹配的准确性；同时，本方案中的视觉信息仅用于建立视觉特征而无需建立视觉地图，省去了建立视觉地图、更新视觉地图耗时的步骤，利用了激光SLAM快速定位的同时，引入准确的视觉闭环检测。

为解决上述技术问题，本发明采用如下的技术方案：

一种基于多传感器的机器人室内定位及制图系统，该系统包括激光定位、视觉闭环检测和地图优化模块三个部分，其特征在于，其中：

所述激光定位由激光帧间匹配和激光闭环检测两部分组成，其中：激光帧间检测用于获取激光传感器以实现匹配，并将匹配结果用于激光闭环检测；激光闭环检测用于使用当前点和以往走过的点的激光信息进行比较，判断机器人是否已经走过该点，并以此作为地图是否需要更新的依据。

所述视觉闭环检测用于使用当前点和以往走过的点的视觉信息进行比较，判断机器人是否已经走过该点，并以此作为是否需进行激光闭环检测的依据。

所述地图优化模块用于计算预计观测值与通过传感器采集到的实际观测值的偏差，然后对激光闭环检测进行校正，并且更新已有地图。

一种基于多传感器的机器人室内定位及制图系统所实现的一种基于多传感器的机器人室内定位及制图方法，其过程包括以下步骤：

S1：激光帧间匹配，根据激光传感器获取到的两帧数据，建立帧间的对应关系以及转换坐标；

S2：视觉闭环检测，视觉闭环检测使用视觉信息对机器人当前位置进行闭环检测，根据视觉闭环检测的结果决定是否执行激光闭环检测；

S3：激光闭环检测，根据一个检测到的地图碎片以及已有地图集合，计算机器人当前处于位置，并以此判断当前是否处于闭环；

S4：地图优化，根据闭环检测的结果迭代调整特征点和摄像头位姿。

进一步地，本发明的步骤S1，具体过程描述如下：

(1)对于当前机器人位姿图像中的每一点，都找出离他最近的线段，称为目标线段，并对应保存起来；

(2)找到一个当前最优的变换。这个最优的变换可以使当前图像中的所有点到它的目标线段的距离的平方和最小；

(3)根据第二步中找出的最优变换来变换当前图像中的所有点，以及旋转当前地图；

(4)重复上面(1)到(3)的步骤，直到第二步中的结果小于一个阈值。

附图说明

图1是本发明的一个具体实施例的结构示意图。

图2为本发明的一个具体实施方式特征精确匹配的方法流程图。

具体实施方式

下面结合附图及具体实施例对本发明进行更加详细与完整的说明。可以理解的是，此处所描述的具体实施例仅用于解释本发明，而非对本发明的限定。

参见图1，一种基于多传感器的机器人室内定位及制图系统，该系统包括激光定位1、视觉闭环检测2和地图优化模块3，其中：

所述激光定位1由激光帧间匹配1-1和激光闭环检测1-2两部分组成，其中：激光帧间检测1-1用于获取激光传感器4以实现匹配，并将匹配结果用于激光闭环检测1-2；激光闭环检测1-2用于使用当前点和以往走过的点的激光信息进行比较，判断机器人是否已经走过该点，并以此作为地图是否需要更新的依据。

所述视觉闭环检测2使用单目摄像头5采集当前点和以往走过的点的视觉信息进行比较，判断机器人是否已经走过该点，并以此作为是否需进行激光闭环检测1-2的依据。

所述地图优化模块3接收来自激光定位1的计算观测值与通过激光传感器4采集到的实际观测值的偏差，然后对激光闭环检测1-2进行校正，并且更新已有地图。

一种基于多传感器的机器人室内定位及制图系统所实现的一种基于多传感器的机器人室内定位及制图方法，其过程包括以下步骤：

S1：激光帧间匹配，根据激光传感器获取到的两帧数据，建立帧间的对应关系以及转换坐标，具体描述如下：

计算出当前帧中每一个变换后的目标点到变换后的目标线段的距离：当前最优变换用平移向量t和转角θ来表示，当前帧中第i个目标点为v_i，为所有目标点的重心，第i条目标线段的正交单位向量为u_i。

在目标线段上任取一点z，将z围绕按以下变换来旋转θ角度，则z如公式1所示：

$z=\left[\begin{array}{cc}cos\mathrm{\θ}& -sin\mathrm{\θ}\\ sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right](z-\stackrel{\‾}{v})+\stackrel{\‾}{v}---\left(1\right)$

当机器人的激光传感器采样频率较高时，转角θ可以近似为极小，则z可以变换如公式2所示：

$z=\left[\begin{array}{cc}1& -\mathrm{\θ}\\ \mathrm{\θ}& 1\end{array}\right](z-\stackrel{\‾}{v})+\stackrel{\‾}{v}=\mathrm{\θ}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right](z-\stackrel{\‾}{v})+z---\left(2\right)$

对于目标点v_i同时如果考虑加入平移因子t，则变换后的目标点v_i如公式3如下：

$v_i=t+\mathrm{\θ}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right](v_i-\stackrel{\‾}{v})+v_i---\left(3\right)$

则变换后的目标点到变换后的目标线段的距离d如公式4所示：

d＝|u_i·(v_i-z)|² (4)

计算出当前帧中的所有目标点到目标线段的距离，寻找使得累加距离最小的变换，即是满足激光帧间匹配的最优变换。

S2：视觉闭环检测，视觉闭环检测使用视觉信息对机器人当前位置进行闭环检测，根据视觉闭环检测的结果决定是否执行激光闭环检测，具体步骤如下：

(1)参见图2，根据激光帧间检测的匹配结果进行特征精确匹配，从匹配的局部特征集合中选取任意一对局部特征，用于计算要匹配的图像与目标图像的基础矩阵F，具体做法如下：

设M为三维空间中一点，在左右两幅图像上的投影分别为m₁和m₂，C₁和C₂为两相机的光心，C₁C₂连线与左右两幅图像分别交于点e₁和e₂，称为对极点。其中投影点m₁和m₂与基础矩阵F满足下面的关系。在代数上，这一几何关系可以表示为公式5所示：

$\begin{array}{c}{m_2}^T{\mathrm{Fm}}_1=0\\ Fe=0\end{array}---\left(5\right)$

根据上述描述，两个匹配的局部特征相当于m₁和m₂，则可以根据公式5计算出基础矩阵F。

(2)将集合中剩余的局部特征对用于校验基础矩阵，如能满足的将该局部特征对作为内点保留，同时计算要匹配的图像与目标图像的能量函数，具体做法如下：

如果剩余的局部特征对与所述基础矩阵相匹配，则满足对极几何约束关系，约束关系描述为：一幅图像上的点m₁，在另一幅图像上的匹配点m₂位于该点的对极线Fm₁上；相反的，一幅图像上的点m₂，在另一幅图像上的匹配点m₁位于该点的对极线F^Tm₂上。这个描述可以形式化为公式6，其中d²表示空间中点到直线的欧式距离。

$\left\{\begin{array}{c}{d}^2(m_1,{\mathrm{Fm}}_2)=0\\ d^2(m_2,F^T{m}_1)=0\end{array}\right.---\left(6\right)$

根据上述描述，将剩余的局部特征对代入公式6，如能满足则将该局部特征对作为内点保留。过滤完所有的局部特征对后，将所有的内点用于计算要匹配的图像与目标图像的能量函数，如公式7所示：

$E(I_1,I_2)=\frac{1}{\frac{1}{2K}\underset{i=1}{\overset{K}{\Σ}}\left(d^2\right(m_{1i},{\mathrm{Fm}}_{2i})+d^2(m_{2i},F^T{m}_{1i}\left)\right)+\mathrm{\β}}---\left(7\right)$

参数β是归一化能量函数的值，能量函数E(I₁，I₂)表示两幅图像I₁和I₂的兴趣点对应集合与估计的基础矩阵F之间的一致程度。当能量函数最大时，表示两幅图像的兴趣点对应的几何约束程度最高。

(3)从匹配的局部特征集合中重新选取任意多对局部特征，重复上述流程(1)和(2)并计算基础矩阵；

(4)多次循环后，比较每次循环的基础矩阵能量函数和内点个数，保留最优基础矩阵F，并用该最优基础矩阵过滤匹配的局部特征集合，实现局部特征的精确匹配。

根据视觉精确匹配的结果，判断当前机器人位置是否处于之前经过的位置，如果是，则输出闭环结果，否则该位置并非闭环位置，则无需进行下一步激光闭环检测操作。

S3：激光闭环检测，根据一个检测到的地图碎片r以及已有地图集合m_i∈m，计算机器人当前处于位置l的先验概率p(l|r，m)，选取出先验概率最大的位置作为机器人当前位置，并依此判断机器人是否实现闭环，具体步骤如下：

根据贝叶斯法则，先验概率p(l|r，m)表示如公式8所示：

$p\left(l\right|r,m)=\frac{p\left(r\right|l,m)p(l,m)}{p(r,m)}---\left(8\right)$

将地图分割为多个单元格，对于每个单元格m_i∈m计算机器人出现在当前单元格的概率p(m_i)，则p(r|l，m)可以近似的表示为公式9：

$p\left(r\right|l,m)=\underset{i}{\Σ}p\left(r\right)\·p\left(m_i\right)---\left(9\right)$

由于地图m是以前已经构建好的，p(l，m)可以简化为p(l)，在匹配区域上是均匀分布的，可以用均匀分布概率来表示；p(r，m)则表示为检测到的地图碎片r以及已有地图m的概率，则p(r，m)＝1；则先验概率p(l|r，m)可用公式10表示：

$p\left(l\right|r,m)=p\left(l\right)\·\underset{i}{\Σ}p\left(r\right)\·p\left(m_i\right)---\left(10\right)$

选取出先验概率最大的位置作为机器人当前位置，并依此判断机器人是否实现闭环。

S4：地图优化，根据闭环检测的结果迭代调整特征点和摄像头位姿，具体过程如下：

机器人在全局下的坐标c_i用公式11表示为：

$c_i={\[t_i^T,{\mathrm{\θ}}_i\]}^T---\left(11\right)$

其中，i＝1，2，…，n表示第i个机器人位姿在世界坐标系的平移的转置，θ_i，i＝1，2，…，n表示第i个机器人位姿在世界坐标系的方向，n表示图中所有机器人位姿的个数。则对于任意两个点c_i和c_j，两点相对距离h(c_i，c_j)用公式12表示为：

$h(c_i,c_j)=\left[\begin{array}{c}{R}_i^T(t_j-t_i)\\ {\mathrm{\θ}}_j-{\mathrm{\θ}}_i\end{array}\right]---\left(12\right)$

其中，表示方向的θ_i旋转矩阵的转置，z_ij表示点c_i和c_j的测量距离，则距离误差表示为e_ij≡z_ij-h(c_i，c_j)，距离误差的约束函数用公式13表示为：

$F(c,e)=\underset{i,j}{\Σ}{e}_{ij}^T{\mathrm{\Λe}}_{ij}---\left(13\right)$

最小化公式13的约束函数用于查找坐标c优化的估计值，采用LM方法进行求解，具体过程如下：

(1)引入函数：

(H+λ·H_diag)Δc＝J^TΛe (14)

其中，H＝J^TΛe，Λ＝diag(Λ_ij)，e表示坐标c的误差，J表示求导，H表示矩阵，H_diag表示矩阵H对角线上的函数值构造的一个对角矩阵，λ表示一个较小的正系数，Δc表示增量，Λ_ij表示测量距离协方差的平方。

(2)初始化λ和H，分别赋予初值。

(3)用梯度下降法求解公式14。

(4)根据求解结果更新Δc，如果误差e减小，则将λ的值缩小一半，并更新坐标为c＝c+Δc；如果误差e增大，则将λ的值增大一倍，仍然保留当前姿态c。

该发明方案的有益效果在于，用单目视觉信息来辅助进行激光SLAM室内定位，利用单目视觉方法来检测闭环并给出一个闭环检测信号帮助解决激光闭环检测成功率过低的问题，提出使用对极几何约束过滤错误匹配的视觉局部特征，提高了视觉局部特征的匹配准确率，保证了视觉匹配的准确性；同时，本方案中的视觉信息仅用于建立视觉特征而无需建立视觉地图，省去了建立视觉地图、更新视觉地图耗时的步骤，利用了激光SLAM快速定位的同时，引入准确的视觉闭环。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (3)

1.一种基于多传感器的机器人室内定位及制图系统，该系统包括激光定位、视觉闭环检测和地图优化模块三个部分，其特征在于，其中：

所述激光定位由激光帧间匹配和激光闭环检测两部分组成，其中：激光帧间检测用于获取激光传感器以实现匹配，并将匹配结果用于激光闭环检测；激光闭环检测用于使用当前点和以往走过的点的激光信息进行比较，判断机器人是否已经走过该点，并以此作为地图是否需要更新的依据。

所述视觉闭环检测用于使用当前点和以往走过的点的视觉信息进行比较，判断机器人是否已经走过该点，并以此作为是否需进行激光闭环检测的依据。

所述地图优化模块用于计算预计观测值与通过传感器采集到的实际观测值的偏差，然后对激光闭环检测进行校正，并且更新已有地图。

2.根据权利要求1所述的一种基于多传感器的机器人室内定位及制图系统所实现的一种基于多传感器的机器人室内定位及制图方法，其特征在于，过程包括以下步骤：

S1：激光帧间匹配，根据激光传感器获取到的两帧数据，建立帧间的对应关系以及转换坐标；

S2：视觉闭环检测，视觉闭环检测使用视觉信息对机器人当前位置进行闭环检测，根据视觉闭环检测的结果决定是否执行激光闭环检测；

S3：激光闭环检测，根据一个检测到的地图碎片以及已有地图集合，计算机器人当前处于位置，并以此判断当前是否处于闭环；

S4：地图优化，根据闭环检测的结果迭代调整特征点和摄像头位姿。

3.根据权利要求1所述的一种基于多传感器的机器人室内定位及制图系统所实现的一种基于多传感器的机器人室内定位及制图方法，其特征在于，步骤S1，具体过程描述如下：

(1)对于当前机器人位姿图像中的每一点，都找出离他最近的线段，称为目标线段，并对应保存起来；

(2)找到一个当前最优的变换。这个最优的变换可以使当前图像中的所有点到它的目标线段的距离的平方和最小；

(3)根据第二步中找出的最优变换来变换当前图像中的所有点，以及旋转当前地图；

(4)重复上面(1)到(3)的步骤，直到第二步中的结果小于一个阈值。