【技术实现步骤摘要】
本专利技术涉及一种基于分层优化与卡尔曼滤波的多模态里程计方法，属于机器人同步定位与建图技术。
技术介绍
1、同时定位与建图(slam)是智能机器人处于未知环境中时，依靠自身搭载的各类传感器采集周围环境信息，实现对自身位置和姿态的计算，同时建立高精度环境地图的技术。
2、目前广泛应用的主要有激光slam和视觉slam两大类。激光雷达在工作时，通过扫描环境信息获得点云，点云中的这些点包含了各种物体特征信息，如物体的反射强度、距离和角度。每个点中的信息存在微弱差异，slam算法对比每个点的信息来获得距离和角度的变化，然后通过计算距离和角度的变化估计出传感器的位姿变化，从而推导出移动设备的位姿变化。相较于视觉slam，激光点云的计算量小，计算简便，深度信息测量精度高，而且在外界光照条件剧烈变化时不易受影响。但是，其缺点是成本昂贵且维护困难，而且在移动设备高速运动过程中会产生畸变，而且由于激光点云缺乏纹理信息导致激光slam闭环检测能力较弱。视觉slam主要依赖各类相机，如彩色相机、灰度相机、事件相机等获取丰富的环境视觉信息，因此可以构建出更符合人类认知的环境地图。相较于激光slam，视觉slam对环境的描述更加精细，可利用环境的纹理信息，提高了辨识精确度，可以从像素层面对比两处环境的差异度。相机成本较低，易于部署在无人机和低成本自动驾驶车辆上，但是由于相机视场角小，对环境光照条件敏感，难以获取准确的深度信息和尺度信息，在快速变化场景下容易出现图像模糊导致信息缺失，因此视觉slam方案通常只能用于光照条件理想的小尺度场景。p>
技术实现思路
1、为了克服现有技术中存在的不足，本专利技术目的是提供一种基于分层优化与卡尔曼滤波的多模态里程计方法，本专利技术方法采用激光雷达点云数据、彩色相机图像数据和惯性测量单元(imu)数据预估智能移动机器人的位姿变换信息，在保证预估精度的同时，大幅提高里程计总体的输出帧率，由原来与雷达数据相同的帧率(10hz)提高到与imu数据相同的帧率(100hz)。在数据集kitti室外场景数据上进行实验，实现低于其他常见里程计算法的平均旋转误差和平移误差。
2、为了实现上述专利技术目的，本专利技术采取的技术方案是：一种基于分层优化与卡尔曼滤波的多模态里程计方法，包括以下步骤：
3、步骤1、进行彩色图像数据和对应的激光点云数据的融合，将雷达坐标系下的三维点云投影到对应时刻的彩色图像上，以获得彩色三维点云；p＝[x,y,z]t表示激光坐标系下原始点云中的一个点，q＝[u,v]t是该点在图像坐标系中的投影；它们的齐次坐标分别为和根据针孔相机模型，和之间的关系通过公式(1)进行描述，
4、
5、其中，s为任意尺度因子，a为彩色相机的内参矩阵，[r t]为激光雷达和彩色相机的外参矩阵；雷达坐标系和相机坐标系之间的几何映射关系，通过公式(2)的单应性矩阵h进行描述，
6、
7、通过公式(3)进行描述，
8、
9、对每一帧三维点云中所有激光点都通过单应性矩阵h进行投影，找到与之对应的投影像素点，将激光点进行染色，获取彩色三维点云p＝{pt|1≤t≤m},m为彩色三维点云的帧数，每一帧彩色三维点云其中，为一帧彩色三维点云中的一个彩色激光点，和表示投影像素点的三维坐标，和表示投影像素点的三原色，n为彩色三维点云中彩色激光点的个数；
10、步骤2、数据预处理，对彩色三维点云的边界进行裁剪以去除边界点和噪声点；对于一个彩色激光点若满足且则保留这个彩色激光点；对彩色三维点云中不包含关键特征的地面部分也进行裁剪，对于一个彩色激光点若满足则保留这个彩色激光点；通过最远点采样算法对裁剪后的彩色三维点云进行降采样以减小彩色三维点云的数据量，并使每一帧彩色三维点云中点的数量统一为8192；
11、步骤3、构建基于分层优化与卡尔曼滤波的多模态里程计，基于分层优化与卡尔曼滤波的多模态里程计主要由分层优化网络和误差状态卡尔曼滤波器(eskf)两部分组成；分层优化网络包括以下几部分：特征提取子网络、多层双向lstm子网络、位姿估计子网络和位姿优化子网络；eskf包括eskf预测和eskf更新两部分；里程计网络的输入为相邻两帧彩色点云数据及之间的惯性测量单元(imu)数据，输出为位姿信息四元数ht＝[at,bt,ct,dt]t和平移向量tt＝[txt,tyt,tzt]t，其中，at是四元数的实部，bt,ct,dt是四元数的虚部，txt,tyt,tzt分别是平移向量t在x,y,z轴上的三个分量；
12、首先，由特征提取子网络分层提取彩色三维点云不同层级下的特征，由双向lstm子网络提取imu时序特征；然后，位姿估计子网络将以上特征拼接后初步估计出位姿信息，并由位姿优化子网络分层优化；最后，eskf通过imu运动学模型计算高频位姿信息，并利用来自位姿优化子网络的输出结果校正高频位姿信息；具体包括以下子步骤：
13、(a)构建特征提取子网络，构建一个孪生特征金字塔网络逐层提取彩色三维点云数据不同密度下的特征；特征提取子网络包含三个结构相同的层；第一层的输入为相邻两帧彩色三维点云pt和pt+1及其特征ft和ft+1，其中，pt和pt+1的大小都为8192×6，ft和ft+1为空数组；在第一层中，首先采用最远点采样算法对pt和pt+1进行降采样，得到第一层彩色三维点云pt1和第一层中降采样的点数设为2048，所以pt1和的大小都为2048×6；然后利用k近邻算法找到pt1中的每个彩色激光点在pt中的k个邻点，以及中的每个彩色激光点在pt+1中的k个邻点，得到第一层彩色三维点云pt1和的邻域信息和第一层中邻点数k设为32，所以和的大小都为2048×32×6；将pt1和的邻域信息和分别输入到三层mlp中提取特征；mlp中每一层神经元的个数分别为(8,8,16)，mlp每一层特征的尺寸分别为(2048×32×8,2048×32×8,2048×32×16)，然后使用动态路由对所有特征向量加权求和来完成池化操作，得到大小均为2048×16的邻域特征ft1和第一层的输出为第一层彩色三维点云pt1和以及它们对应的特征ft1和特征提取子网络的第二层和第一层的结构相同，只有部分参数不同；第二层的输入为第一层输出的彩色三维点云pt1和以及它们对应的特征ft1和第二层降采样的点数设为1024，邻点数k设为32，mlp中每一层神经元的个数分别为(16,16,32)，第二层的输出为第二层彩色三维点云pt2和以及它们对应的特征ft2和其中，pt2和的大小都为1024×6，ft2和的大小都为1024×32；同样的，第三层的输入为第二层输出的彩色三维点云pt2和以及它们对应的特征ft2和降采样的点数设为256，邻点数k设为16，mlp中每一层神经元的个数分别为(32,32,64)，第三层的输出为第三层彩色三维点云pt3和以及它们对应的特征ft3和其中，pt3和的大小都为256×6，ft3和的大小都为256×64；
14、(b)构建多层双向l本文档来自技高网...
【技术保护点】
1.一种基于分层优化与卡尔曼滤波的多模态里程计方法，其特征在于包括以下步骤：
【技术特征摘要】
1.一种基于分层优化与卡尔曼滤波的多模...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。