基于混合特征的全景惯性SLAM方法
本技术涉及同步定位与建图,特别是涉及基于混合特征的全景惯性slam方法。
背景技术:
1、随着人工智能、计算机视觉和机器人技术的发展,智能驾驶汽车、无人机和巡检机器人等自主系统正逐渐进入复杂开放环境,并承担感知、导航、避障、巡检和自主决策等任务。对于这类自主机器人而言,稳定可靠的位姿估计是实现环境理解与自主行动的基础:机器人需要根据相机、惯性测量单元、激光雷达等传感器获取的外部观测信息,结合自身运动模型建立观测方程和运动方程,从而连续估计自身在三维空间中的位置、姿态和运动状态,并在此基础上完成地图构建、路径规划、运动控制和任务执行。
2、视觉同步定位与建图(visual simultaneous localization and mapping,vslam)因相机成本低、信息密度高、语义表达能力强,成为自主机器人定位与导航中的重要技术路线。vslam 通常利用图像中的局部特征、相邻帧几何约束、关键帧地图和闭环检测来估计载体运动并构建环境地图。视觉惯性同步定位与建图(visual-inertialsimultaneous localization and mapping, vi-slam)进一步引入惯性测量单元(inertial measurement unit, imu)信息,通过惯性预积分和视觉—惯性联合优化,提升系统在尺度恢复、姿态估计、短时快速运动和弱纹理场景下的稳定性。典型方法包括 orb-slam3、vins-mono、openvins 等,它们已经在针孔相机、鱼眼相机和双目相机条件下形成了较完整的前端跟踪、局部建图、重定位、闭环检测和后端优化框架。
3、现有技术中,同步定位与建图一般包括以下五种方式:
4、1、视觉或视觉惯性 slam 系统建立在有限视场成像模型之上,观测范围受相机视场角限制。
5、2、基于erp图像的单目全景视觉里程计或全景slam方法,例如openvslam和360vo。
6、3、引入imu测量的全景视觉惯性里程计方法,例如360-vio。
7、4、采用学习型特征或深度网络增强视觉slam前端的方法。
8、5、面向全景图像的学习型位姿估计或深度视觉里程计方法,例如panopose和360dvo。
9、但是,现有方法虽然分别在有限视场视觉惯性slam、erp单目全景视觉slam、全景视觉惯性里程计、学习型特征匹配以及深度视觉里程计等方面取得了一定效果,但仍未形成一种面向erp全景图像的统一视觉惯性slam方案。具体而言,现有技术至少存在以下不足:一是有限视场相机方法在快速转向、遮挡和弱纹理场景中容易因可见特征不足而退化;二是单目erp全景方法缺少imu约束,难以保证真实尺度和短时运动稳定性;三是部分全景视觉惯性方法缺少完整闭环检测和全局一致性优化,难以消除长期累计漂移;四是学习型特征方法多基于针孔图像假设,且未考虑极区畸变;五是传统特征与学习型特征混合使用时缺少类别约束和统一几何度量,容易引入错误匹配和闭环误检。
10、因此,有必要提出一种面向erp全景图像的视觉惯性同步定位与建图方法。
技术实现思路
1、基于此,有必要针对上述技术问题,提供一种基于混合特征的全景惯性slam方法,能够在统一全景成像模型下同时增强前端特征约束、视觉惯性优化约束和闭环一致性约束,从而提高无人机、移动机器人和复杂空间巡检场景下定位建图的鲁棒性、精度和全局一致性。
2、基于混合特征的全景惯性slam方法,包括:
3、获取一个时序的历史信息,得到每一帧同步的等距柱状投影图像和惯性测量数据;
4、根据等距柱状投影图像,建立三维空间点投影到等距柱状投影图像坐标的正向投影模型以及等距柱状投影图像坐标到单位球面视线的逆投影模型,得到全景成像模型;
5、根据当前帧的等距柱状投影图像,分别提取传统手工特征和学习型特征,计算每个特征的纬度畸变权重函数,得到混合特征数据;
6、根据上一帧与当前帧的混合特征数据与惯性测量数据,建立初始全景地图;
7、根据初始全景地图,确定候选匹配关系和候选匹配特征;对具有候选匹配关系的两个候选匹配特征,当特征类别相同时,得到当前帧的估计位姿;
8、若当前帧是关键帧,建立关键帧之间的共视关系,得到当前帧的共视关键帧集合;根据全景成像模型以及当前帧的估计位姿,得到更新后的全景地图;
9、根据更新后的全景地图和共视关键帧集合,构建全景视觉惯性位姿图联合优化问题,并根据纬度畸变权重函数求解,得到初始解;
10、若当前帧是关键帧且所有关键帧的数量大于预设阈值时,设置闭环约束,加入全景视觉惯性位姿图联合优化问题,重新求解并结合初始解,得到当前帧的优化关键帧状态和优化地图点状态;遍历一个时序的每一帧信息,实现全景惯性slam。
11、在一个实施例中,三维空间点投影到等距柱状投影图像坐标的正向投影模型,包括:
12、
13、式中,为三维空间点在单位球面上的经度角,为三维空间点在单位球面上的纬度角,为等距柱状投影图像像素坐标的水平坐标,为等距柱状投影图像像素坐标的垂直坐标,为平方根运算,为三维空间点在全景相机坐标系下沿x轴的坐标,为三维空间点在全景相机坐标系下沿y轴的坐标,为三维空间点在全景相机坐标系下沿z轴的坐标,为等距柱状投影图像宽度,为等距柱状投影图像高度。
14、在一个实施例中,等距柱状投影图像坐标到单位球面视线的逆投影模型,包括:
15、
16、式中,为等距柱状投影图像像素在全景相机坐标系下对应的单位视线方向,为从三维空间点投影到 erp 图像坐标的投影函数。
17、在一个实施例中,计算每个特征的纬度畸变权重函数,包括:
18、
19、式中,为第帧中第个特征的纬度畸变权重,为图像帧的索引,为特征的索引,为第帧中第个特征的垂直像素坐标,为图像金字塔相邻层之间的尺度因子,为特征所在金字塔层。
20、在一个实施例中,根据上一帧与当前帧的混合特征数据与惯性测量数据,建立初始全景地图,包括:
21、根据上一帧等距柱状投影图像的混合特征数据与当前帧等距柱状投影图像的混合特征数据,执行帧间特征匹配,以建立全景混合特征匹配约束;
22、根据上一帧的惯性测量数据与当前帧的惯性测量数据,建立imu积分约束;
23、根据全景混合特征匹配约束以及imu积分约束进行判断,当等距柱状投影图像的视差、全景混合特征匹配的数量或imu积分的数值满足预设条件时,根据全景混合特征匹配约束以及imu积分约束,对关键帧进行计算,得到关键帧状态;根据关键帧状态,得到地图点到当前帧的深度;根据地图点到当前帧的深度以及逆投影模型,建立多个地图点;
24、根据等距柱状投影图像的视差、全景混合特征匹配的数量、imu积分的数值以及所有地图点,建立初始全景地图。
25、在一个实施例中,根据初始全景地图,确定候选匹配关系和候选匹配特征;对具有候选匹配关系的两个候选匹配特征,当特征类别相同时,得到当前帧的估计位姿,包括:
26、根据初始全景地图,进行候选匹配,确定上一帧等距柱状投影图像的任意特征与当前帧等距柱状投影图像的任意特征是否存在候选匹配关系,并将具有候选匹配关系的两个特征作为候选匹配特征;
27、根据候选匹配关系,建立当前帧候选匹配特征与对应局部地图点之间的候选二维三维关联关系,并建立上一帧候选匹配特征和当前帧候选匹配特征的候选二维二维关联关系;
28、对具有候选匹配关系的两个候选匹配特征,当特征类别相同时,根据两个候选匹配特征,计算匹配评分;根据匹配评分进行判断,当匹配评分满足预设阈值时,进行几何一致性检验;若通过几何一致性检验,判定候选二维三维关联关系和候选二维二维关联关系为有效二维三维关联关系和有效二维二维关联关系,以有效二维三维关联关系和有效二维二维关联关系为有效关系;根据有效关系,得到当前帧的估计位姿。
29、在一个实施例中,若当前帧是关键帧,建立关键帧之间的共视关系,得到当前帧的共视关键帧集合;根据全景成像模型以及当前帧的估计位姿,得到更新后的全景地图,包括:
30、根据当前帧进行判断,若当前帧是关键帧,统计不同关键帧共同观测的地图点数量,作为共视权重;根据共视权重,建立关键帧之间的共视关系,以与当前帧存在共视关系的关键帧为共视关键帧;
31、根据全景成像模型,分别将上一帧中的像素坐标和当前帧中的像素坐标反投影为单位球面方向向量,并结合上一帧的估计位姿和当前帧的估计位姿,得到候选新地图点;
32、对候选新地图点进行有效性检查,将通过检查的候选新地图点加入初始全景地图,得到更新后的全景地图;
33、按照当前帧与每个共视关键帧之间共视权重由高到低的顺序,对共视关键帧进行排序,并选取预设数量的共视关键帧,构建当前帧的共视关键帧集合。
34、在一个实施例中,根据更新后的全景地图和共视关键帧集合,构建全景视觉惯性位姿图联合优化问题,并根据纬度畸变权重函数求解,得到初始解,包括:
35、根据更新后的全景地图、共视关键帧集合以及预先标定的相机外参,构建全景视觉惯性位姿图联合优化问题;
36、根据纬度畸变权重函数,调整视觉观测的协方差矩阵,并根据协方差矩阵,求解全景视觉惯性位姿图联合优化问题,得到初始解。
37、在一个实施例中,构建全景视觉惯性位姿图联合优化问题,包括:
38、
39、其中,
40、
41、
42、式中,为共视关键帧集合的状态,包含第1个共视关键帧的状态至第个共视关键帧的状态;为更新后的全景地图中的地图点;为相邻共视关键帧和之间的imu积分的残差;为参与联合优化问题的地图点数量;为能够观测到的第个地图点的共视关键帧集合;为huber鲁棒核;为第个地图点在第个共视关键帧中的全景重投影残差;为第个地图点在第个共视关键帧中实际观测像素的横坐标,为根据第个共视关键帧的预测位姿和第个地图点通过erp正向投影模型得到的预测像素的横坐标,为第个地图点在第个共视关键帧中实际观测像素的纵坐标,为据第个共视关键帧的预测位姿和第个地图点通过erp正向投影模型得到的预测像素的纵坐标;为第个共视关键帧中第个特征的纬度畸变权重函数;为二维单位矩阵。
43、在一个实施例中,若当前帧是关键帧且所有关键帧的数量大于预设阈值时,设置闭环约束,加入全景视觉惯性位姿图联合优化问题,重新求解并结合初始解,得到当前帧的优化关键帧状态和优化地图点状态;遍历一个时序的每一帧信息,实现全景惯性slam,包括:
44、若当前帧是关键帧且所有关键帧的数量大于预设阈值时,区分每个关键帧的传统手工特征和学习型特征,然后依次进行候选检索和得分融合,以判断每个关键帧是否属于候选闭环关键帧,得到所有候选闭环关键帧;
45、采用全景成像模型,对每个候选闭环关键帧,进行闭环验证;在闭环验证通过后,对通过闭环验证的候选闭环关键帧,设置闭环约束;
46、将闭环约束加入当前帧的全景视觉惯性位姿图联合优化问题,重新求解全景视觉惯性位姿图联合优化问题,并对初始解进行更新,得到当前帧的优化关键帧状态和优化地图点状态;
47、遍历一个时序的每一帧信息,得到每一帧的优化关键帧状态和优化地图点状态,实现全景惯性slam。
48、上述基于混合特征的全景惯性slam方法,在保留通用视觉惯性 slam 关键帧地图结构和实时运行能力的基础上,建立全景成像模型,引入传统手工特征与学习型特征的协同表达、构建全景混合特征匹配约束与imu积分约束、进行混合特征匹配、联合优化、闭环检测,从而解决现有方法在 erp 边界存在误差、弱纹理跟踪、混合特征误匹配、模块间几何度量不一致以及低视差弱激励初始化不稳定等方面的问题,提高全景视觉惯性 slam 在无人机、移动机器人和狭窄复杂空间巡检等场景下的跟踪鲁棒性、地图稳定性和闭环可靠性,并提高位姿估计的准确性。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com