一种基于视觉与激光slam的机器人室内建图方法及系统与流程

本发明涉及巡检机器人领域，具体涉及一种基于视觉与激光slam的机器人室内建图方法及系统。

背景技术：

室内导航环境的好坏，直接影响机器人的各种巡检任务的完成。要完成地图的创建，首先要获取周围环境的信息，这就要用到传感器来完成。现在机器人的制图研究热点大体上集中在两种传感器上，一种是激光传感器，激光传感器具有解析度高、测距精度高、抗有源干扰能力强、探测性能好、不受光线影响，缺点是获得的信息不具备语义信息，配准精度不高，建图过程中常存在闭环检测失败的情况。另一种的视觉传感器，可以获得丰富的纹理信息，具有匹配准确的优点，但是存在受光照变化影响大的缺点。

为了克服激光传感器和视觉传感器的缺陷，本发明提出一种基于视觉与激光slam的机器人室内建图方法及系统。

技术实现要素：

为了解决上述问题，本发明提出了一种基于视觉与激光slam的机器人室内建图方法，包括以下步骤：

视觉传感器获取左视图和右视图，生成特征点点云，具体包括：

提取左视图和右视图的特征点，

对获得的特征点进行匹配，

计算相匹配的特征点的空间坐标；

计算基于视觉的机器人的位姿变换估计，即将本时刻左视图和右视图相匹配的特征点与本时刻的上一时刻的左视图和右视图相匹配的特征点进行二次匹配，根据二次匹配的特征点的坐标变化和特征点的主方向变化获得机器人的位姿变换关系，将特征点的描述子投影到对应的地图栅格上，定义栅格的特征点描述子属性；

获得激光数据，计算基于激光的机器人位姿变换估计、更新地图栅格的置信度；

采用瓦片式的建图方法，利用连续的激光点云数据生成子图；

闭环检测，即将获得的特征点的描述子与每张子图的每个栅格对应的栅格的特征点描述子属性进行匹配；

图优化，具体包括：通过图的节点表示机器人的位姿，通过连接节点的边表示机器人位姿间的变换关系，计算各机器人位姿与经变换矩阵变换后的估计位姿之间的误差，计算带有若干条边的图的误差目标函数，通过多次迭代求得使目标函数收敛的机器人位姿，用于更新各个子图中的机器人位姿，即可构成最终的建图结果。

进一步地，所述提取左视图和右视图的特征点包括：

利用orb算法对左右视图进行fast角点检测，提取特征点p；

建立以特征点p为原点的坐标系，在领域s内计算质心位置c，以特征点p为起点，质心c为终点构造向量领域s的矩可表示为：

其中y(x,y)为图像的灰度值，x,y∈[-r,r]，r为邻域s的半径，p、q取0或1，则该邻域的质心位置为：

特征点p的主方向θ为：

θ＝arctan2(m01，m10)；

生成特征点描述子：

在特征点p的邻域s内，产生随机n对点集，用2×n的矩阵来表示：特征点的主方向θ和对应的旋转矩阵rθ，计算旋转后的2×n的矩阵：

其中，

则特征点p的邻域s内二进制测试准则可定义为：

其中，iθ＝i1θ,…,inθ，jθ＝j1θ,…,jnθ，τ表示一种二进制测试准则，表示点iθ的灰度值，表示点jθ的灰度值；

此时可得到特征点p旋转的描述子：

k′为128、256或512，对应描述符的大小分别为16字节、32字节或64字节，根据方法的实时性、存储空间以及识别效率进行选择。

进一步地，所述对获得的特征点进行匹配具体为：

计算左右视图特征点的描述子的汉明距离d进行匹配：

其中，fl,fr分别为左视图和右视图的某个特征点的描述子；当两帧图像中存在汉明距离小于阈值的两个特征点时，则两个特征点匹配成功。

进一步地，所述计算相匹配的特征点的空间坐标具体为：

其中，(x,y,z)为特征点p在空间中的坐标，(ul,vl)为特征点p在左侧相机中的图像点坐标,(ur,vr)为右侧相机中的图像点坐标,f焦距为双目相机的焦距，d为两个相机之间的距离。

进一步地，所述采用瓦片式的建图方法，利用连续的激光点云数据生成子图具体为：

利用连续的激光点云数据生成子图，每连续ts内数据生成一张子图，第二张子图包含上一张子图的后t(t<t)s内的数据。

进一步地，所述图优化具体为：

机器人的位姿被表示成图中的节点，观测信息经过处理后转变为机器人之间位姿间的约束关系，并通过连接节点间的边来表示；图的每个节点取决于机器人位姿的描述，所有节点的状态方程为其中分别为全局坐标系下的机器人的位姿，边则描述了机器人位姿之间的变换关系；节点pi和节点pj之间观测方程可以表示为：

zj＝xiti,j

其中，xi为节点pi相应的机器人在全局坐标系下的位姿，xj为节点pj相应的机器人在全局坐标系下的位姿，zj为节点pj的观测值，即xi经变换矩阵变换后的估计位姿，ti,j为节点pi与节点pj之间的变换关系；

机器人位姿xi与经变换矩阵变换后的估计位姿zj之间存在误差e(zj,xj)，计算公式为：

e(zj，xj)＝xiti，j-xj

带有若干条边的图的误差目标函数e(x)为：

其中，信息矩阵ωk是协方差矩阵的逆，是一个对称矩阵；它的每个元素ωi,j用作误差的系数，对e(zi,xi)、e(zj,xj)的误差相关性进行预计；

假设xk增加一个增量δx，通过多次迭代最终求得满足目标方程e(x)的收敛值，用求得的δx更新状态变量x，x中的各个节点的位姿即可构成最终的建图结果。

一种基于视觉与激光slam的机器人室内建图系统，包括前端和后端，系统的前端利用双目视觉传感器获取环境的左视图和右视图，通过orb算法提取左右视图，并对左右视图的特征点进行匹配，进而求取各个特征点在空间内的坐标；对相邻时刻获得的特征点进行匹配，进而求得时刻机器人的位姿变换关系，将特征点投影到地图栅格上，定义有特征点投影的栅格地图具有描述子属性；系统的前端通过激光传感器获得环境的激光点云数据，并对激光点云数据进行去噪处理，通过求取机器人位姿初值、定义扫描窗口、求取置信度获得机器人的位姿变换估计，将激光点云投射到栅格地图上，更新各个地图栅格的置信度；系统后端对得到的地图进行闭环检测和图优化。

与现有技术相比，本发明具有以下有益效果：

采用视觉和激光相结合的建图方法，一方面改善了视觉算法受光照影响大的缺点，另一方面利用特征点的描述子属性检测闭环，解决了激光闭环检测成功率过低的问题，提高了闭环检测效率。

附图说明

图1为基于视觉与激光slam的机器人室内定位与建图流程。

图2为m16模板示意图。

具体实施方式

下面结合附图，对一种基于视觉与激光slam的移动机器人室内定位与建图方法进行详细描述。

一种基于视觉与激光slam的移动机器人室内定位与建图方法，其流程如图1所示。具体而言，基于视觉与激光slam的移动机器人室内定位与建图系统分为前端和后端，系统的前端利用双目视觉传感器获取环境的左视图和右视图，通过orb算法提取左右视图，并对左右视图的特征点进行匹配，进而求取各个特征点在空间内的坐标；之后对相邻时刻获得的特征点进行匹配，进而求得时刻机器人的位姿变换关系，将特征点投影到地图栅格上，定义有特征点投影的栅格具有描述子属性。同时，系统的前端通过二维激光传感器获得环境的激光点云数据，并对激光点云数据进行去噪处理，并通过求取机器人位姿初值、定义扫描窗口、求取置信度等方式获得机器人的位姿变换估计，在此基础上将激光点云投射到栅格地图上，更新各个地图栅格的置信度。最后为了减小传感器的累计误差，系统后端对得到的地图进行闭环检测和图优化。

一、系统前端

1、基于orb的特征点点云生成

(1)特征点提取：双目视传感器可同时获得环境的左视图il和右视图ir，利用orb(orientedfastandrotatedbrief，一种快速特征点提取和描述的算法)算法对左右视图进行fast(featuresfromacceleratedsegmenttest)角点检测，提取特征点，并计算特征点的主方向，生成ofast，具体流程如下：

①首先对得到的左视图和右视图进行灰度化处理：y＝0.39×r+0.5×g+0.11×b，y为灰度化后某像素点的灰度值，r、g、b为该像素点红、绿、蓝三个颜色分量。

②在左视图中，以像素点p为圆心，在半径向下取整为3的圆周内，有16个像素点(m10模板)，如图2所示。若m16中有连续n个(9≤n≤12)点的灰度值均大于yp+t或均小于yp-t(其中yp为点p的灰度值，t为阈值)，则判定点p为特征点。(以p为圆心画一个半径为3pixel的圆。圆周上如果有连续n个像素点的灰度值比p点的灰度值大或者小，则认为p为特征点。一般n设置为12。)

③建立以特征点p为原点的坐标系，在领域s内计算质心位置c，以特征点p为起点，质心c为终点构造向量领域s的矩可表示为：

其中y(x,y)为图像的灰度值，x,y∈[-r,r]，r为邻域s的半径，p、q取0或1，则该邻域的质心位置为：

特征点p的主方向θ为：

θ＝arctan2(m01，m10)

(2)特征点描述子生成：在特征点p的邻域s内，产生随机n对点集，用2×n的矩阵来表示：根据步骤③求出的特征点的主方向θ和对应的旋转矩阵rθ，计算旋转后的2×n的矩阵：

其中，

则特征点p的邻域s内二进制测试准则可定义为：

其中，iθ＝i1θ,…,inθ，jθ＝j1θ,…,jnθ，τ表示一种二进制测试准则，表示点iθ的灰度值，表示点jθ的灰度值；

此时可得到特征点p旋转的描述子：

k′可为128,256,或512，对应描述符的大小分别为16字节、32字节和64字节，根据算法的实时性、存储空间以及识别效率进行选择。

(3)特征点匹配：按照上述方案获得到左右视图两帧图像特征点的描述子以后，计算二者的汉明距离d进行匹配：

其中，fl,fr分别为左视图和右视图的某个特征点的描述子。当两帧图像中存在汉明距离小于阈值的两个特征点时，则两个特征点匹配成功。

特征点空间位置计算：假设特征点p在左侧相机中的图像点坐标为(ul,vl)，右侧相机中的图像点坐标为(ur,vr)，则特征点p在空间中的坐标(x,y,z)的计算方式如下：

其中f焦距为双目相机的焦距，d为两个相机之间的距离，求出所有的相匹配的特征点坐标，此时的特征点具有唯一的空间坐标和唯一的描述子。

2、机器人位姿变换估计

将激光数据进行处理得到激光点云数据，对激光点云数据进行过滤，即去掉激光点云数据中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点。计算有效激光点云在激光传感器坐标系中的位姿。

(1)观测值为激光时的机器人位姿变换估计、更新地图栅格的置信度

①机器人位姿初值估计

设定获得各帧激光数据的时间点是计算位姿的时间点。假定机器人在时间点t的位姿是p(t)，前一个计算位姿的时间点为t-1，相应的位姿是p(t-1)，下一个计算位姿的时间点t+1，相应的的位姿是p(t+1)。通过时间点t和t-1之间的位姿差(包括位移和旋转角度)来计算机器人运动的线速度v(x,y)和角速度w。

线速度v(x,y)＝(t和t-1之间的位移)/时间差

角速度w＝(t和t-1之间的旋转角度)/时间差

根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值pose_estimated；

②定位扫描窗口

机器人的定位扫描用位移扫描参数和角度扫描参数，位移扫描参数用于限定定位扫描窗口的位移范围，以位姿的估计值pose_estimated为中心，上下左右各偏离lcm大小的正方形。角度扫描参数用于限定定位扫描窗口的角度范围，以位姿的估计值为中心角度，上下各偏离w度的角度。

定位扫描窗口大小确定定位扫描窗口内各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿possible_candidates。

④置信度估计

根据每个候选位姿对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度candidate_probability，公式如下：

其中，m为某个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设其中第n个栅格的地图坐标为(xn,yn)，该栅格置信度是取值范围为0.1～0.9。

根据每个候选位姿possible_candidate与位姿的估计值pose_estimated的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，transweight是位移权重，candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rotweight是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重candidate_weight的乘积作为当前位姿的置信度分值，公式如下，

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿更新位姿的估计值pose_estimated，作为最优位姿估计p(t+1)。根据获得的最优位姿变换估计p(t+1)和机器人当前的位姿p(t)可求得机器人位姿变换c(包括旋转矩阵和和位移矩阵)，机器人根据变换关系t移动到p(t+1)的位姿，且在此基础上将当前的点云数据投影到对应位置地图栅格当中。若其中有重复落在同一地图栅格位置的多个点，仅取其中一个点对应的地图栅格在地图坐标系中的坐标，根据激光点云的投影情况更新栅格的概率。第一帧激光点投影到地图栅格上以后，各个栅格更新后的置信度如下：

当第二帧激光数据落到二维栅格平面时，根据以下公式更新各个栅格的置信度：

各个栅格的状态更新系数为：

当第n帧激光点云数据落入二维平面栅格时，各个栅格的置信度为：

其中，pn-1(xi,yi)为第n-1帧激光点云数据落入二维平面栅格时，坐标为(xi,yi)的栅格的置信度；

(2)观测值为特征点时的机器人位姿变换估计

假设t-1时刻根据左右视图获得的相匹配的特征点序列为s＝[s1,s2,…,sg]，t时刻根据左右视图获得的相匹配的特征点序列为p＝[p1,p2,…,ph]，将t-1时刻和t时刻的特征点序列s、p进行匹配，此时根据相匹配的特征点的坐标变化和特征点的主方向变化可以获得机器人的位姿变换关系t′，从而在此基础上将特征点的描述子投影到对应的地图栅格上，此时定义栅格的第二属性，即特征点描述子属性。此时栅格地图的置信度具有激光概率和特征点两重属性，第一重属性和第二重属性相互独立。

4、生成子图

采用瓦片式的建图方法，利用连续的激光点云数据生成子图，每连续ts内数据生成一张子图，第二张子图包含上一张子图的后t(t<t)s内的数据。如：将第1s到40s内的数据进行配准形成子图a1，第21s到60s的数据进行配准形成子图a2，第41s到80s的数据进行配准形成子图a3，……。

二、系统后端

1、闭环检测

根据当前获得的左视图和右视图，利用orb算法提取特征点，并计算特征点的描述子。将获得的特征点的描述子存入集合q,q＝{q1,q2,…,qn}，假设此时已生成m张子图，将q中每个描述子与每张子图中的每个栅格对应的第二层属性，即栅格特征点描述子进行匹配。当集合q中的描述子与某张子图的栅格特征点描述子匹配成功率达到90％以上时，闭环检测成功。

2、图优化

图优化是通过减少轨迹中的漂移减少轨迹扭曲，在图优化的过程中，机器人的位姿被表示成图中的节点，观测信息经过处理后转变为机器人之间位姿间的约束关系，并通过连接节点间的边来表示。图的每个节点取决于机器人位姿的描述，所有节点的状态方程为其中分别为全局坐标系下的机器人的位姿，边则描述了机器人位姿之间的变换关系节点pi和节点pj之间观测方程可以表示为zj＝xiti,j，即机器人经过运动ti,j由pi运动到pj，其中，xi为节点pi相应的机器人在全局坐标系下的位姿，，xj为节点pj相应的机器人在全局坐标系下的位姿，zj为节点pj的观测值，即xi经变换矩阵变换后的估计位姿，ti,j为节点pi与节点pj之间的变换关系。

在优化过程中，每个节点都有一个估计值，那么由一条边连接的两个顶点各有一个估计值，它们与这条边的约束之间会有一个误差。也就是说，理想情况下我们有：

zj＝xiti,j

机器人位姿xi与经变换矩阵变换后的估计位姿zj之间存在误差e(zj,xj)：

e(zj,xj)＝xiti,j-xj

由于每条边都产生了一点点误差，假设建立的是一个带有k条边的图，其目标函数可以写成：

其中，信息矩阵ωk是协方差矩阵的逆，是一个对称矩阵。它的每个元素ωi，j用作误差的系数，对e(zi，xi)、e(zj，xj)的误差相关性进行预计。最简单的是把ωk设成对角矩阵，对角矩阵元素的大小表明对各元素相应误差的重视程度。采用其他协方差矩阵也可以。

xk增加一个增量δx，误差值则从e(zk，xk)变为e(zk，xk+δx)，对误差项进行一阶展开：

jk是e(zk，xk)关于xk的导数，矩阵形式下为雅可比阵。对于第k条边的目标函数项，进一步展开有：

e(xk+δx)＝e(zk，xk+δx)^tωke(zk，xk+δx)

≈[e(zk，xk)+jkδx]^tωk[e(zk，xk)+jkδx]

＝e(zk，xk)^tωke(zk，xk)+2e(zk，xk)^tωkjkδx+δx^tjk^tδx

＝ck+2bkδx+δx^thkδx

将与δx无关的项整理成常数项ck，δx的一次项系数整理成bk，δx二次项系数写成hk。

重写目标函数：

其中xk发生增量后，目标函数e(x)项的改变值为：

δe(x)＝2bδx+δx^thδx

为了使δe(x)取得最小值，令δe(x)对δx的导数为零，有

用求得的δx更新状态变量x，通过多次迭代最终求得满足目标方程的状态变量x的收敛值，对整个运动过程中机器人位姿进行优化，x中的各个节点的位姿即可构成最终的建图结果。

本发明采用视觉和激光相结合的建图方法，一方面改善了视觉算法受光照影响大的缺点，另一方面利用特征点的描述子属性检测闭环，解决了激光闭环检测成功率过低的问题，提高了闭环检测效率。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。