申请/专利权人：新疆大学

申请日：2024-01-27

公开（公告）日：2024-04-26

公开（公告）号：CN117928561A

主分类号：G01C21/20

分类号：G01C21/20;G01C21/16;G01C21/00;G01S17/86;G01S17/89;G01C22/00;G06T7/70;G06T5/50;G06T7/12;G06N3/0442;G06V10/44;G06V10/80;G06V10/82;G06N3/084;G06N3/0499;G06N3/045

优先权：

专利状态码：在审-实质审查的生效

法律状态：2024.05.14#实质审查的生效;2024.04.26#公开

摘要：本发明属于机器人同步定位与建图技术领域，一种基于分层优化与卡尔曼滤波的多模态里程计方法，包括以下步骤：1进行彩色图像数据和对应的激光点云数据的融合，2数据预处理，对彩色三维点云的边界进行裁剪以去除边界点和噪声点，3构建基于分层优化与卡尔曼滤波的多模态里程计，4训练多模态里程计网络，并预测相邻两帧彩色三维点云之间的位姿。本发明方法与其它常见的算法相比，本发明提出的多模态融合里程计在数据集各个序列上均具有更低水平的旋转误差和平移误差，最终实现了0.66的平均平移误差和0.41的平均旋转误差，优于其它对比的方法。

主权项：1.一种基于分层优化与卡尔曼滤波的多模态里程计方法，其特征在于包括以下步骤：步骤1、进行彩色图像数据和对应的激光点云数据的融合，将雷达坐标系下的三维点云投影到对应时刻的彩色图像上，以获得彩色三维点云；p＝[x,y,z]T表示激光坐标系下原始点云中的一个点，q＝[u,v]T是该点在图像坐标系中的投影；它们的齐次坐标分别为和根据针孔相机模型，和之间的关系通过公式1进行描述， 其中，s为任意尺度因子，A为彩色相机的内参矩阵，[Rt]为激光雷达和彩色相机的外参矩阵，雷达坐标系和相机坐标系之间的几何映射关系通过公式2的单应性矩阵H进行描述， 便可通过公式3进行描述， 对每一帧三维点云中所有激光点都通过单应性矩阵H进行投影，找到与之对应的投影像素点，将激光点进行染色，获取彩色三维点云P＝{Pt|1≤t≤m},m为彩色三维点云的帧数，每一帧彩色三维点云其中，为一帧彩色三维点云中的一个彩色激光点，和表示投影像素点的三维坐标，rti,和表示投影像素点的三原色，n为彩色三维点云中彩色激光点的个数；步骤2、数据预处理，对彩色三维点云的边界进行裁剪以去除边界点和噪声点；对于一个彩色激光点若满足且则保留这个彩色激光点；对彩色三维点云中不包含关键特征的地面部分也进行裁剪，对于一个彩色激光点若满足则保留这个彩色激光点；通过最远点采样算法对裁剪后的彩色三维点云进行降采样以减小彩色三维点云的数据量，并使每一帧彩色三维点云中点的数量统一为8192；步骤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轴上的三个分量；首先，由特征提取子网络分层提取彩色三维点云不同层级下的特征，由双向LSTM子网络提取IMU时序特征；然后，位姿估计子网络将以上特征拼接后初步估计出位姿信息，并由位姿优化子网络分层优化；最后，ESKF通过IMU运动学模型计算高频位姿信息，并利用来自位姿优化子网络的输出结果校正高频位姿信息，具体包括以下子步骤：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；b构建多层双向LSTM子网络提取IMU时序特征，使用两个独立的通道分别提取角速度测量值ω和加速度测量值a的时序特征，每个通道包含三层双向LSTM层Bi-LSTM；其中，ωt＝[ωxt,ωyt,ωzt]，at＝[axt,ayt,azt]为t时刻的一帧IMU测量数据，每次连续输入10帧数据，所以角速度测量值ω和加速度测量值a的大小均为3×10；由于IMU测量中噪声方差很大，因此在Bi-LSTM层之后加入Dropout层来防止网络过拟合，rate为0.2；然后将从加速度测量值和角速度测量值中提取的特征信息利用两层Bi-LSTM层和一层Dropout层映射为高维特征FtI；双向LSTM子网络的输出为IMU时序特征FtI；c构建位姿估计子网络，位姿估计子网络的输入为IMU时序特征FtI、特征提取子网络第三层输出的彩色三维点云Pt3和以及它们对应的特征Ft3和首先进行特征融合；位姿估计子网络采用K近邻算法选取彩色三维点云Pt3和中每个激光点的k个邻点，并将这些激光点及其邻点的特征拼接为一个整体特征通过公式4进行描述， 其中，fi表示彩色三维点云Pt3中第i个点pi在特征Ft3中对应的特征，fij表示pi的k个邻点在特征Ft3中对应的特征；表示pi和其邻点的位置信息的拼接，通过公式5进行描述， 其中，pij表示pi的k个邻点，{pij-pi}表示pi和这些邻域点的相对坐标，{||pij-pi||}表示pi和这些邻域点的欧式距离；表示特征向量拼接；使用两个MLP处理整体特征一个计算融合特征F1，另一个计算每个点的权重W1，其中，MLP中每一层神经元的个数分别为64,64,128，池化后F1和W1的大小均为256×128；将F1和W1的对应元素相乘便得到了这一层点云的融合特征大小为256×128；接下来进行位姿估计；将融合特征IMU的时序特征FtI以及点云Pt3的位置信息拼接并使用FPS和KNN进行降采样和分组，对采样结果同样分别使用两次MLP计算特征F2和W2，池化后大小均为256×64；将F2和W2的对应元素相乘后分别输入到MLP和全连接层FC，其中，一个FC的输出层大小为4，用于求解四元数另一个FC的输出层大小为3，用于求解平移向量位姿估计子网络的输出为融合特征四元数和平移向量d构建位姿优化子网络，3层位姿优化子网络充分利用彩色点云在不同层级下的特征信息，分层优化网络估计的位姿，从而提高里程计整体的输出精度；三层位姿优化子网络结构相同，第一层位姿优化子网络的输入为特征提取子网络中的第二层彩色三维点云Pt2和以及它们对应的特征Ft2和位姿估计子网络的输出四元数平移向量和融合特征通过输出位姿和对Pt2进行空间变换得到然后利用步骤3c中特征融合的方法对Ft2和进行特征融合操作，得到其中，的大小为256×128；将和进行拼接，然后通过三层MLP对拼接结果进行编码得到融合特征大小为256×128；将和进行拼接，然后利用步骤3c中位姿估计的方法进行位姿估计，得到残差位姿和通过将残差位姿与四元数和平移向量相加，得到四元数和平移向量第二层和第三层位姿优化子网络与第一层的结构相同；第二层位姿优化子网络的输入为第一层的输出四元数平移向量和融合特征以及特征提取子网络的第一层彩色三维点云Pt1和以及它们对应的特征Ft1和输出为融合特征四元数和平移向量其中，的大小为1024×128；第三层位姿优化子网络的输入为第二层的输出四元数平移向量和融合特征以及特征提取子网络中原始相邻两帧彩色三维点云Pt和Pt+1以及它们对应的特征Ft和Ft+1，输出为最终的位姿计算结果四元数ht和平移向量tt；e实现ESKF预测和更新，ESKF根据IMU数据预测机器人状态，并利用分层优化网络输出的位姿信息更新预测的机器人状态以消除累计误差；基于已有的运动学模型，ESKF利用上一时刻的IMU数据和机器人状态预测机器人当前时刻的状态，若分层优化网络未及时提供位姿信息四元数ht和平移向量tt，ESKF则直接将预测的机器人当前状态输出；若分层优化网络提供了四元数ht和平移向量tt，ESKF基于卡尔曼更新方程利用分层优化网络提供的位姿信息对预测的机器人当前状态进行修正后再输出；然后ESKF继续利用当前时刻的IMU数据和机器人状态预测机器人下一时刻的状态，并重复迭代上述过程；分层优化网络的输出频率为10Hz，ESKF预测的频率与IMU数据输入频率一致，为100Hz，即ESKF每10次预测进行一次更新，使ESKF预测的位姿保持较高频率的同时精度更高；ESKF预测和更新模块的输出为100Hz的四元数和平移向量。步骤4、训练多模态里程计网络，并预测相邻两帧彩色三维点云之间的位姿，将预处理后的彩色三维点云输入到已构建的神经网络中进行训练；网络模型通过Tensorflow1.15搭建；在训练过程中，采用Adam优化器，其中，第一衰减指数β1和第二衰减指数β2分别设置为0.9和0.999；训练时的批量大小设置为4；学习率设置为0.001；最大Epoch为300；网络训练完毕后，使用最终的模型对相邻两帧彩色三维点云之间的位姿进行预测。

全文数据：

权利要求：

百度查询： 新疆大学 基于分层优化与卡尔曼滤波的多模态里程计方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD