买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网恭喜浙江工业大学申请的专利一种基于激光与视觉异步融合的SLAM算法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN114742874B 。

龙图腾网通过国家知识产权局官网在2025-04-29发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202210313473.1，技术领域涉及：G06T7/579；该发明授权一种基于激光与视觉异步融合的SLAM算法是由邢科新;张兴盛;董为;李伟腾;慈雯祺;蒲茜设计研发完成，并于2022-03-28向国家知识产权局提交的专利申请。

本一种基于激光与视觉异步融合的SLAM算法在说明书摘要公布了：本发明公开了一种基于激光与视觉异步融合的SLAM算法，首先将传感器的数据按较快的传感器采样频率划分为m个采样区间，每个采样区间内会存在两种情况：1只拥有相机的位姿估计；2同时拥有来自激光传感器和视觉传感器的位姿估计。结合Kalman滤波器及多传感器异步融合的方法，得到了改进后的基于激光与视觉异步融合位姿估计算法。为了有效利用每个传感器不同的特性，根据机器人的运行状态，引入基于角度的加权系数，提高测量的置信度。该方法可以更好地利用视觉传感器中的冗余数据，提高算法的跟踪精度。同时，将视觉传感器姿态估计作为激光传感器姿态估计的先验，减少迭代次数，提高算法的效率。

本发明授权一种基于激光与视觉异步融合的SLAM算法在权利要求书中公布了：1.一种基于激光与视觉异步融合的SLAM算法，其特征在于，包括以下步骤：步骤1对于由视觉传感器反馈得到的位姿增量，根据相机小孔成像原理以及刚体变换公式，假设空间点P在世界坐标系下的齐次坐标为P＝XW,YW,ZW,1T；则其在相机的像素坐标系下的坐标为： 其中u,v为点p在像素坐标系下的坐标值；s为点p在相机坐标系下的深度值；K为相机的内参矩阵；R为旋转矩阵；t为平移矩阵；步骤2通过ORB特征提取算法得到当前帧中图像的特征点，将rBRIEF描述符得到的当前帧中特征点的描述符与上一时刻图像中的特征点进行描述符之间的匹配，建立对应的匹配关系；将上一时刻的匹配点通过相机的内参矩阵以及初始位姿投影到当前帧图像中；由于相机的位姿通常存在偏差，所以投影点与当前地图上的匹配点之间通常会存在偏差；假设特征点P在当前帧图像中的坐标为P＝u1,v1T，对应上一帧中的匹配点在当前帧中的匹配点坐标为P’＝u2,v2T；通常由于相机位姿的偏差导致这两个点的坐标并不相同，因此定义误差函数： 其中e是误差值，u,v是当前帧中特征点的像素坐标，P是当前帧特征点对应的匹配点；将其改写为李代数形式： 其中expδ为相机坐标系的李代数表达式，e为空间点P投影到当前图像中与匹配点之间的投影误差；通常一帧图像中会存在若干个匹配点，根据所有的匹配点得到投影误差函数： 其中ui表示当前帧中第i个特征点；Pi表示其对应的匹配点的世界坐标；si表示该点的深度；K表示相机的内参矩阵；expδ表示相机坐标系的李代数表达式；步骤3利用高斯牛顿法或者列文伯格-马夸尔特方法求解公式4，得到位姿估计为： 其中z1k1为视觉传感器在k1时刻的最优估计，R为旋转矩阵，t为平移矩阵；v1k1为相机估计噪声且假设服从高斯分布；步骤4假设k2时刻得到的激光传感器数据为令Siδ2为世界坐标，MSiδ2为坐标点Siδ2的占用概率，为MSiδ2的导数；MSiδ2与的值可通过对世界坐标Siδ2进行双线性插值求得；那么机器人从上一时刻到当前时刻的位姿增量可由下式给出： 其中 步骤5根据位姿增量得到机器人在激光传感器位姿估计下的坐标为：z2k2＝z2k2-1+Δδ2+v2k28其中z2k2为激光传感器在k2时刻的最优位姿估计；z2k2-1为其在上一时刻的最优位姿估计；v2k2为观测噪声且假设服从高斯分布，满足如下关系： 其中viki，vjTkj为测量噪声，E代表均值，δij，为克罗内克函数，Riki为k时刻来自视觉或者激光传感器的协方差矩阵；步骤6采用了异步融合方式对来自两个传感器的数据进行融合；首先将传感器的数据按较快的传感器采样频率划分为m个采样区间，每个采样区间内会存在两种情况：1只拥有相机的位姿估计；2同时拥有来自激光传感器和视觉传感器的位姿估计；结合Kalman滤波器及多传感器异步融合的方法，得到了改进后的基于激光与视觉异步融合位姿估计算法如下： 其中为根据上一时刻机器人位姿的最优估计得到的预测位姿；P0k|k为预测位姿的协方差矩阵；ziki为k时刻来自视觉或者激光传感器的最优位姿估计；Rik为k时刻来自视觉或者激光传感器的协方差矩阵，下标i代表传感器数据的个数；当采样区间内只有相机的数据时，i＝1，此时式10变为： 此时估计模型转变为以较快的传感器采样频率下对单个传感器数据进行位姿估计的Kalman滤波器；当采样区间内同时存在相机的数据以及激光传感器的位姿估计结果时，此时式10变为式12、13的形式： 此时估计模型变为以较慢的传感器采样速率下对激光传感器与视觉传感器进行融合估计的Kalman滤波算法；算法以上一时刻的最优估计作为预测量，首先融合当前时刻传感器一估计的位姿，然后再融合传感器二估计的位姿结果，最终以两次融合后的传感器估计值作为最终位姿估计的结果；步骤7为了有效利用每个传感器不同的特性，引入置信度概念；使用置信度w表示对两个传感器位姿估计的置信度，将式10改为： 其中wi代表第i个传感器的位姿估计置信度；步骤8传感器的特性通常与机器人运动过程相关，因此wi应与机器人的运动状态相关；当机器人运动沿直线行驶时，算法增大对视觉的置信度；当机器人出现旋转时，算法增大对激光的置信度；每次融合时算法会根据激光传感器最新的位姿估计判断机器人当前运动过程中是否发生旋转；其由式6得到，这里记为Δα2；将角度变化量代入wi得到： 其中Δα2为上一时刻到当前时刻估计的机器人旋转角度；根据当前机器人运行效果，每个采样周期内机器人旋转角度不超过0.25rad，因此式15修改为： 将式16代入式14得到：

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人浙江工业大学，其通讯地址为：310014 浙江省杭州市拱墅区潮王路18号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。