一种面向单调无纹理大场景的多传感器融合slam方法
技术领域
1.本发明涉及移动机器人技术领域，尤其是涉及一种面向单调无纹理大场景的多传感器融合slam方法。
背景技术：2.随着工业技术的现代化、自动化与智能化发展，越来越多的场景需要机器人和无人车的参与，同时定位与地图构建(slam)成为了无人车自主导航和定位的关键技术，让机器人在未知环境下能够在移动过程中基于其传感器(如激光雷达、深度相机等)创建环境地图，从而实现机器人的自主定位和导航。
3.基于激光雷达的机器人导航技术离不开定位技术的实现，定位的前提是需要一个清晰的环境地图，现有的slam技术在小范围场景能够进行高精度的建图与定位，但在大场景弱纹理的环境下，会发生建图的不清晰以及定位的不准确等问题，尤其对于以摄像头为主要传感器的slam系统，因此，需要提出一种面向单调无纹理大场景环境下的多传感器融合slam方法。
技术实现要素：4.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种面向单调无纹理大场景的多传感器融合slam方法。
5.本发明的目的可以通过以下技术方案来实现：
6.一种面向单调无纹理大场景的多传感器融合slam方法，用以减少在单调无纹理大场景下雷达定位产生的漂移，包括以下步骤：
7.s1、获取3d激光雷达、imu传感器、深度相机和轮式编码器的信息；
8.s2、将深度相机采集到的每一帧图像通过图像金字塔提取orb特征点，并实现特征点之间的匹配，根据匹配结果获取深度相机的位姿估计值；
9.s3、监听imu与轮式编码器的信息，通过扩展卡尔曼滤波进行位姿融合，获取更新后的位姿矩阵和协方差矩阵；
10.s4、将imu与轮式编码器融合后的位姿与深度相机的位姿估计值进一步融合，并根据视觉里程计提取的orb特征点数量确定融合的动态权重；
11.s5、通过将3d激光雷达扫描到的点云进行分类匹配，再通过最小二乘计算出雷达当前位姿并得到去畸变点云；
12.s6、对去畸变点云进行进一步分类匹配，得到准确的位姿，并构建全局地图。
13.所述的步骤s1具体为：
14.根据3d激光雷达的实际参数和应用要求获得激光点云并进行滤波，将深度相机的像素信息和深度信息分为两个通道进行提取。
15.所述的步骤s2具体为：
16.对于orb特征点中的fast角点，首先采用图像金字塔进行角点的提取以保证尺度
特性，并在提取过程中保证特征点在每一层金字塔上均匀分布以提高系统的鲁棒性和稳定性，然后通过灰度质心法获取角点方向，计算特征点的brief描述子并通过hamming距离进行匹配，最后采用光束平差法，构建相机位姿关于重投影误差的最小二乘问题，使用列文伯格-马尔夸特法迭代求解相机位姿。
17.采用图像金字塔提取角点的计算式为：
[0018][0019]
其中，nm为金字塔第m层所需提取的特征点数量，n为所需提取特征点的总数，s为金字塔相邻层之间的缩放因子，m为金字塔总层数。
[0020]
为克服无纹理环境中难以提取到特征点的影响，提出指标δ以衡量视觉里程计的准确度，则指标δ的表达式为：
[0021][0022]
其中，n_featurepoints为特征点提取的最大数量，n为提取到的特征点数量。
[0023]
所述的步骤s3具体为：
[0024]
首先启动ros节点并监听轮式编码器和imu发布的信息，构建扩展卡尔曼滤波器，进行状态更新，以获取的轮式编码器信息作为观测量和协方差矩阵并进行状态更新，并发送更新后的状态变量和协方差矩阵；然后获取imu信息作为观测量和协方差矩阵并根据轮式编码器发送和状态变量和协方差矩阵进行状态更新；最后发布更新后的状态变量和协方差矩阵作为融合后的imu里程计信息。
[0025]
所述的步骤s4中，在将视觉里程计与机器人自身运动里程计融合时，根据orb特征点数量确定融合的动态权重，则有：
[0026]
当视觉里程计提取到的特征点数量n＞80时：
[0027][0028]
当视觉里程计提取到的特征点数量n＜30时：
[0029][0030]
当视觉里程计提取到的特征点数量30＜n＜80时：
[0031][0032]
其中，t为融合后的位置向量，q为融合后的姿态四元数，t
camera
为相机的位置向量，q
camera
为相机的姿态四元数，t
imu
为imu里程计的位置向量，q
imu
为里程计的姿态四元数，进而根据动态权重输出融合后的位姿。
[0033]
所述的步骤s5中，通过计算每个特征点的平面光滑度，将其分类为平面点和边缘
点，通过计算点到线距离匹配边缘点，通过计算点到面距离匹配平面点，得到雷达关于距离的最小二乘函数，进行迭代优化，得到雷达位姿，并通过将点云数据重投影到初始时刻消除雷达运动产生的点云畸变。
[0034]
所述的平面光滑度c的表达式为：
[0035][0036]
其中，雷达坐标系被设定为l，lk表示第k个周期时的雷达观测坐标系，x
l(k,i)
为第k个周期扫描到的第i个点云，s为当前点所在的集合，
[0037]
所述的步骤s6中，特征点的进一步分类匹配过程与步骤s5相同，通过在地图上存储点云到一个立方体空间中，提取出与当前点云相交的特征点，计算特征点周围集合的协方差矩阵，并计算协方差矩阵的特征值和特征向量，根据特征值和特征向量判断点为平面点或边缘点，采用平面点计算点到平面距离、边缘点计算点到线距离的方法，非线性优化采用列文伯格-马尔夸特法进行优化，最后采用像素网格滤波器对地图进行优化使得点云分布更加均匀。
[0038]
与现有技术相比，本发明具有以下优点：
[0039]
一、本发明融合了视觉里程计和imu里程计，能够解决在空旷环境下，如长走廊、地下车库环境中激光雷达由于扫描不到足够多的特征点从而定位不准确的问题，由于融合了多个传感器，也提高了系统的鲁棒性。
[0040]
二、雷达的位姿估计方法简单高效，本发明通过将点云进行分类避免了处理所有点云中所有点，提高了slam方法的运算速度，更加节省运算空间。
[0041]
三、本发明的部署较为简单，且可以调整融合的相关参数，能够应对多种实际场景进行灵活调整。
附图说明
[0042]
图1为本发明slam方法的整体框架图。
[0043]
图2为视觉里程计的运行流程。
[0044]
图3为imu融合轮式编码器的流程图。
具体实施方式
[0045]
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
[0046]
如图1所示，本发明提供一种面向单调无纹理大场景的多传感器融合slam方法，该方法包括四个部分：
[0047]
(1)前端：包括传感器与环境的交互，激光雷达通过扫描周围环境得到点云数据，深度相机通过读取周围环境得到深度图与像素点坐标，imu通过加速度和陀螺仪计算得到小车当前的加速度和角速度，同时，通过构建视觉里程计和将imu与轮式编码器相融合可以分别估算得到相机与imu的位姿，再将二者按照下式进行融合：
[0048]
当视觉里程计提取到的特征点数量n＞80时有：
[0049][0050]
n＜30时有：
[0051][0052]
30＜n＜80时有：
[0053][0054]
其中，t为融合后的位置向量，q为融合后的姿态四元数，t
camera
为相机的位置向量，q
camera
为相机的姿态四元数，t
imu
为imu里程计的位置向量，q
imi
为里程计的姿态四元数，进而根据动态权重输出融合后的位姿。
[0055]
(2)雷达里程计：本发明中雷达的运动估计模型为一个扫描周期中匀速和匀角速度运动，t为当前时间戳，t+1为下个扫描周期的开始，t
k+1l
为雷达在[t
k+1
,t]时段的位姿变换，t
k+1l
包括雷达在6自由度的刚性运动，且t
k+1l
＝[t
x
,ty,tz,θ
x
,θy,θz]
t
,t
x
,ty,tz为雷达坐标系沿着x,y,z轴的位移，θ
x
,θy,θz为右手坐标系下的旋转角，对于p
k+1
中的点i，设ti为当前时间戳，t
(k+1,i)l
为[t
k+1
,ti]的坐标变换，t
(k+1,i)l
可以由t
k+1l
线性插值得到：
[0056][0057]
ε
k+1
和h
k+1
为p
k+1
中边缘点和平面点的集合，和为第t
k+1
次扫描中重投影后的点集，为了求解雷达运动，本发明对ε
k+1
和以及h
k+1
和的几何关系进行推导，过程如下：
[0058][0059]
x
l(k+1,i)
是ε
k+1
和中点i的坐标，是和中符合条件的点的坐标，t
(k+1,i)l
(a:b)是t
(k+1,i)l
中从a到b的元素，r是由罗德里格斯公式计算得到的旋转矩阵：
[0060][0061]
得到在ε
k+1
中得到边缘点和特征线的几何关系：
[0062]
fε(x
l(k+1,i)
,t
k+1l
)＝dε,i∈ε
k+1
[0063]
得到h
k+1
中平面点和特征平面间的几何关系：
[0064]
fh(x
l(k+1,i)
,t
k+1l
)＝dh,i∈h
k+1
[0065]
最后，用列文伯格-马尔夸特法求解雷达运动，可得到非线性函数：
[0066]
f(t
k+1l
)＝d
[0067]
f的每一行对应一个特征点，d包括了对应的距离。本发明计算出了f关于t
k+1l
的
jacob矩阵，被记作j，算法为由此问题即转化为通过最小化d来作为非线性最小二乘问题求解：
[0068]
t
k+1l
←
t
k+1l-(j
t
j+λdiag(j
t
j))-1jtd[0069]
其中λ为lm法中的因子。
[0070]
(3)雷达建图：在地图上存储点云qk到一个10m的立方体空间中，立方体中与相交的特征点会被提取出来，并储存在3d的kd-tree中，设s
′
为qk中特征点的周围点的集合，接下来计算s
′
的协方差矩阵，记作m，对于m的特征值和特征向量，记作v和e。如果v包含的一个特征值将显著大于另外两个，则s
′
的集合为边界线，与这个特征值对应的特征向量e也代表了边界线的方向，如果v包含了两个很大的特征值，而另一个十分小，则s
′
的集合为平面，而对应最小的特征值的e也代表了这个平面的方向。
[0071]
(4)位姿转换：通过将雷达里程计和雷达建图环节输出的位姿进行整合，最后输出准确的位姿，使用迭代法进行位姿的优化。
[0072]
图2为视觉里程计的构建流程，包括以下步骤：
[0073]
首先是进行相机的初始化，视觉里程计节点接收相机的内参，得到像素坐标系的平移量以及焦距的信息；然后进行特征点的提取，在相机当前图像下提取orb特征点，设定一个阈值n，当特征点的数量小于n时，代表此时环境缺少特征点，此时的定位是不准确的，这里取n＝30，当n《30时，对相机重新初始化，将当前位姿置为0，当特征点数量大于n时，随着机器人的继续前进，视觉里程计将之前提取到的特征点与当前帧提取到的特征点相匹配，并通过匹配好的特征点对估算得到相机位姿；最后进行点云数据的发布，为基于视觉里程计的地图构建提供基础。
[0074]
特征点的提取和匹配可以分为以下步骤：
[0075]
(1)对当前图像构建图像金字塔；
[0076]
(2)在金字塔的每一层提取fast角点；
[0077]
(3)根据灰度质心法计算出特征点的角度；
[0078]
(4)计算brief描述子；
[0079]
(5)对两帧图像的brief描述子实现匹配。
[0080]
相机的运动估计采用光束平差法(bundle ajustment，简称ba)，ba法是一种通过视觉重建提取出最佳的模型和相机参数的方法。本发明通过将相机的实际投影位置与观测位置的差值进行求和，将误差转换为和相机位姿有关的函数，并通过最小二乘方法对其进行迭代优化得到相机位姿的最优估计值。
[0081]
图3为imu融合轮式编码器的流程，包括以下步骤：
[0082]
(1)启动ros节点并监听轮式编码器和imu发送的信息
[0083]
(2)构建扩展卡尔曼滤波器
[0084]
(3)加入白噪声，进行状态更新
[0085]
(4)获取轮式编码器的信息作为观测量和协方差矩阵并进行状态更新，然后发送更新后的状态变量和协方差矩阵
[0086]
(5)获取imu信息作为观测量和协方差矩阵并根据轮式编码器发送的状态变量和协方差矩阵进行状态更新
[0087]
(6)发布最后更新的状态变量和协方差矩阵，作为融合后的里程计信息，话题为/odom_combined。
技术特征：1.一种面向单调无纹理大场景的多传感器融合slam方法，用以减少在单调无纹理大场景下雷达定位产生的漂移，其特征在于，包括以下步骤：s1、获取3d激光雷达、imu传感器、深度相机和轮式编码器的信息；s2、将深度相机采集到的每一帧图像通过图像金字塔提取orb特征点，并实现特征点之间的匹配，根据匹配结果获取深度相机的位姿估计值；s3、监听imu与轮式编码器的信息，通过扩展卡尔曼滤波进行位姿融合，获取更新后的位姿矩阵和协方差矩阵；s4、将imu与轮式编码器融合后的位姿与深度相机的位姿估计值进一步融合，并根据视觉里程计提取的orb特征点数量确定融合的动态权重；s5、通过将3d激光雷达扫描到的点云进行分类匹配，再通过最小二乘计算出雷达当前位姿并得到去畸变点云；s6、对去畸变点云进行进一步分类匹配，得到准确的位姿，并构建全局地图。2.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s1具体为：根据3d激光雷达的实际参数和应用要求获得激光点云并进行滤波，将深度相机的像素信息和深度信息分为两个通道进行提取。3.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s2具体为：对于orb特征点中的fast角点，首先采用图像金字塔进行角点的提取以保证尺度特性，并在提取过程中保证特征点在每一层金字塔上均匀分布以提高系统的鲁棒性和稳定性，然后通过灰度质心法获取角点方向，计算特征点的brief描述子并通过hamming距离进行匹配，最后采用光束平差法，构建相机位姿关于重投影误差的最小二乘问题，使用列文伯格-马尔夸特法迭代求解相机位姿。4.根据权利要求3所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，采用图像金字塔提取角点的计算式为：其中，n
m
为金字塔第m层所需提取的特征点数量，n为所需提取特征点的总数，s为金字塔相邻层之间的缩放因子，m为金字塔总层数。5.根据权利要求3所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，为克服无纹理环境中难以提取到特征点的影响，提出指标δ以衡量视觉里程计的准确度，则指标δ的表达式为：其中，n_featurepoints为特征点提取的最大数量，n为提取到的特征点数量。6.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s3具体为：
首先启动ros节点并监听轮式编码器和imu发布的信息，构建扩展卡尔曼滤波器，进行状态更新，以获取的轮式编码器信息作为观测量和协方差矩阵并进行状态更新，并发送更新后的状态变量和协方差矩阵；然后获取imu信息作为观测量和协方差矩阵并根据轮式编码器发送和状态变量和协方差矩阵进行状态更新；最后发布更新后的状态变量和协方差矩阵作为融合后的imu里程计信息。7.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s4中，在将视觉里程计与机器人自身运动里程计融合时，根据orb特征点数量确定融合的动态权重，则有：当视觉里程计提取到的特征点数量n＞80时：当视觉里程计提取到的特征点数量n＜30时：当视觉里程计提取到的特征点数量30＜n＜80时：其中，t为融合后的位置向量，q为融合后的姿态四元数，t
camera
为相机的位置向量，q
camera
为相机的姿态四元数，t
imu
为imu里程计的位置向量，q
imu
为里程计的姿态四元数，进而根据动态权重输出融合后的位姿。8.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s5中，通过计算每个特征点的平面光滑度，将其分类为平面点和边缘点，通过计算点到线距离匹配边缘点，通过计算点到面距离匹配平面点，得到雷达关于距离的最小二乘函数，进行迭代优化，得到雷达位姿，并通过将点云数据重投影到初始时刻消除雷达运动产生的点云畸变。9.根据权利要求8所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的平面光滑度c的表达式为：其中，雷达坐标系被设定为l，l
k
表示第k个周期时的雷达观测坐标系，x
l(k,i)
为第k个周期扫描到的第i个点云，s为当前点所在的集合。10.根据权利要求1所述的一种面向单调无纹理大场景的多传感器融合slam方法，其特征在于，所述的步骤s6中，特征点的进一步分类匹配过程与步骤s5相同，通过在地图上存储点云到一个立方体空间中，提取出与当前点云相交的特征点，计算特征点周围集合的协方差矩阵，并计算协方差矩阵的特征值和特征向量，根据特征值和特征向量判断点为平面点
或边缘点，采用平面点计算点到平面距离、边缘点计算点到线距离的方法，非线性优化采用列文伯格-马尔夸特法进行优化，最后采用像素网格滤波器对地图进行优化使得点云分布更加均匀。
技术总结本发明涉及一种面向单调无纹理大场景的多传感器融合SLAM方法，包括以下步骤：S1、获取3D激光雷达、IMU传感器、深度相机和轮式编码器的信息；S2、将深度相机采集到的每一帧图像通过图像金字塔提取ORB特征点，并实现特征点之间的匹配，根据匹配结果获取深度相机的位姿估计值；S3、监听IMU与轮式编码器的信息进行位姿融合，获取更新后的位姿矩阵和协方差矩阵；S4、将IMU与轮式编码器融合后的位姿与深度相机的位姿估计值进一步融合；S5、通过将3D激光雷达扫描到的点云进行分类匹配，得到去畸变点云；S6、对去畸变点云进行进一步分类匹配，并构建全局地图。与现有技术相比，本发明具有提高系统鲁棒性、提高运算速度、部署简单等优点。部署简单等优点。部署简单等优点。
技术研发人员：刘成菊 陈启军 侯哲 石文博
受保护的技术使用者：同济大学
技术研发日：2022.07.08
技术公布日：2022/11/1
