一种多传感器融合的SLAM方法和装置制造方法及图纸

本发明专利技术提供一种多传感器融合的SLAM方法和装置，方案为了扩大视觉传感器观测视野，避免因为遮挡、突发强光照和无明显纹理区域带来的特征点提取失效，本发明专利技术引入了多摄像机；同时，为了消除单目尺度不确定和快速旋转引起的单目退化，将IMU和轮速计的惯导观测与视觉观测紧耦合，以得到一个更加鲁棒的SLAM系统，以使得所述多传感器融合的SLAM装置具有更广的视野范围。视野范围。视野范围。

【技术实现步骤摘要】
一种多传感器融合的SLAM方法和装置


[0001]本专利技术涉及
，具体涉及一种基于多摄像机、IMU和轮速计的多传感器融合的SLAM方法和装置。

技术介绍

[0002]即时定位与地图构建(Simultaneous Localization And Mapping，以下简称SLAM)技术是一种在未知的环境和位置下，运动体以自身传感器的观测作为输入，实时地输出自身的位置和姿态，同时根据位姿增量式构建地图的方法。SLAM技术目前在自动驾驶、高精地图、机器人、无人机和AR/VR等领域都有着重要应用。
[0003]SLAM按照主要传感器的不同可以分为激光雷达为主的激光SLAM和单目、双目、RGBD等相机为主的视觉SLAM。激光SLAM相对已经发展得比较成熟，但是考虑到大规模量产应用的成本，视觉SLAM仍是目前研究的主流。
[0004]对于视觉SLAM来说，双目相机测量范围受到基线和分辨率限制，且需要大量计算才能恢复像素深度，实时性较差；深度相机可以直接得到像素深度，但是价格昂贵、视野小、测量范围窄、光照敏感，目前主要用于室内。单目相机以其体积小、成本低廉、实时性好受到广泛关注，但缺点是需要平移才能确定深度，且本身存在尺度不确定性，导致重建轨迹和真实轨迹之间相差一个缩放因子。目前，单目视觉SLAM有众多优秀的开源框架，如ORB
‑
SLAM系列、LSD
‑
SLAM、DSO、SVO等。
[0005]IMU和轮速计不依赖外界环境，通过预积分能够给出运动体自身的速度和角速度，具有重建的绝对尺度；较高的输出频率可以在图像帧间给出预测结果，根据预测结果能够减小两帧间匹配的搜索范围，避免单目快速旋转时退化导致轨迹跟丢。但与此同时，IMU和轮速计的数据误差通过累积会不断放大，和视觉观测耦合优化可以减少由偏置带来的发散和累积误差，使得结果更加准确。目前，单目视觉融合IMU的SLAM开源框架主要有VINS、OKVINS、VINS
‑
Fusion等，这些方案要求硬件对齐精确，同时初始化较为严格，需要给出尺度、偏置和重力方向。在本专利技术中，引入轮速计可以直接获得速度变量，简化初始化过程。
[0006]在实际应用中，单路相机观测往往视野较为局限，当视野范围出现遮挡、强光照或无明显纹理区域时会导致特征点跟丢，从而引发系统失效。多摄像机系统提供多个视角的观测，扩宽了视野范围，当其中某路摄像机失效时，也能通过其他摄像机继续追踪和建图，大大提高了系统的鲁棒性。Multicol
‑
SLAM提出了基于鱼眼相机模型的多路纯视觉SLAM方法，直接在多个鱼眼相机图像上提取特征点和描述子，容易存在误匹配且实时性较差；同时去掉了鱼眼图像畸变较大的边缘，使得点、线特征的提取、匹配和优化都存在较大困难；它还要求相机视野必须存在视场重叠以确定尺度，使其应用具有局限性。Cubemap SLAM在将单路鱼眼图像用透视立方体模型校正，充分利用图像信息，但没有克服单路视野的局限性，同时单目无法恢复绝对尺度。

技术实现思路

[0007]有鉴于此，本专利技术实施例提供一种基于多摄像机、IMU和轮速计的多传感器融合的SLAM方法，以实现能给定绝对尺度、扩大观测视野的SLAM系统。
[0008]为实现上述目的，本专利技术实施例提供如下技术方案：
[0009]一种多传感器融合的SLAM方法，包括：
[0010]标定SLAM系统中相机的内参；
[0011]标定SLAM系统中相机、轮速计和IMU的外参；
[0012]基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
[0013]控制所述SLAM系统中多摄像机和轮速计松耦合初始化；
[0014]采用追踪线程获取初始化后的地图点和位姿，基于参考帧至当前关键帧获取到的图像数据、IMU数据和轮速计数据计算并优化当前关键帧位姿，将所述关键帧发送至局部建图线程；
[0015]采用局部建图线程获取追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合，筛选并剔除冗余关键帧和冗余地图点；
[0016]采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正。
[0017]可选的，上述多传感器融合的SLAM方法中，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
[0018]可选的，上述多传感器融合的SLAM方法中，基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型，包括：
[0019]基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
[0020]将公式作为相机投影模型，其中，所述p为世界坐标系下的地图点P(X，Y，Z)投影到相机c
i
的像素坐标系下得到的像素点p(x，y)，所述K
j
为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机c
i
的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机c
i
相机坐标系的位姿变换矩阵，所述T
bw
为从世界坐标系到本体坐标系的位姿变换矩阵，P为世界坐标系下的地图点P(X，Y，Z)。
[0021]可选的，上述多传感器融合的SLAM方法中，所述控制所述SLAM系统中多摄像机和轮速计松耦合初始化，包括：
[0022]通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
[0023]进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距
离；
[0024]通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
[0025]基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
[0026]基于所述第一平移距离和第二平移距离计算尺度因子；
[0027]基于所述尺度因子恢复姿态和地图点的绝对尺度，并对所述IMU轮速计进行初始化。
[0028]可选的，上述多传感器融合的SLAM方法中，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正，包括：
[0029]将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多传感器融合的SLAM方法，其特征在于，包括：标定SLAM系统中相机的内参；标定SLAM系统中相机、轮速计和IMU的外参；基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；控制所述SLAM系统中多摄像机和轮速计松耦合初始化；采用追踪线程获取初始化后的地图点和位姿，基于获取到的图像数据、IMU数据和轮速计数据、基于所述图像数据插入关键帧并对所述位姿和地图点进行修正，将所述关键帧发送至局部建图线程；采用局部建图线程获取追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合地图点，筛选并剔除冗余关键帧和冗余地图点；采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正。2.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。3.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型，包括：基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；将公式作为相机投影模型，其中，所述p为世界坐标系下的地图点P(X，Y，Z)投影到相机c
i
的像素坐标系下得到的像素点p(x，y)，所述K
j
为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机c
i
的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机c
i
相机坐标系的位姿变换矩阵，所述T
bw
为从世界坐标系到本体坐标系的位姿变换矩阵，P为世界坐标系下的地图点P(X，Y，Z)。4.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，所述控制所述SLAM系统中多摄像机和轮速计松耦合初始化，包括：通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；基于所述第一平移距离和第二平移距离计算尺度因子；基于所述尺度因子恢复姿态和地图点的绝对尺度，并对所述IMU轮速计进行初始化。5.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，采用回环检测线程对
所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正，包括：将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。6.一种多传感器融合的SLAM装置，其特征在于，包括：标定单元，用于标定SLAM系统中相机的内参；标定SL...

【专利技术属性】
技术研发人员：刘燕霖，陈敏鹤，金忠孝，
申请(专利权)人：上海汽车工业集团总公司，
类型：发明
国别省市：