一种适用于无人船的水面SLAM回环检测与位姿优化方法及系统与流程

摘要: 本发明涉及同步定位与地图构建,尤其涉及一种适用于无人船的水面slam回环检测与位姿优化方法及系统。背景技术:1、无人船在河道巡检、水域测绘等作业中,需要利用同步...
一种适用于无人船的水面SLAM回环检测与位姿优化方法及系统与流程

本发明涉及同步定位与地图构建,尤其涉及一种适用于无人船的水面slam回环检测与位姿优化方法及系统。

背景技术:

1、无人船在河道巡检、水域测绘等作业中,需要利用同步定位与地图构建技术确定船体位姿并维护环境地图;一种已有处理方式是从连续观测中提取局部特征,估计相邻帧运动,再将当前观测与历史观测匹配,利用回环约束校正累计漂移。在岸堤形态重复的河段,不同位置可能出现相似的局部角点或轮廓;水面反光、漂浮物及船体随波运动还会改变观测外观和帧间配准结果,使局部外观相似不能直接证明无人船返回同一位置。如果仅依据局部匹配结果建立回环约束,而未结合岸线整体形态、局部几何一致性及约束误差进行校验,不同河段之间的错误对应就可能进入位姿优化,牵动历史关键帧位置,造成地图岸线错位。因此,在具有重复岸线和水面扰动的场景中,如何降低错误回环约束对位姿优化及地图一致性的影响,是需要解决的主要技术问题。

技术实现思路

1、鉴于此,本发明实施例提供了一种适用于无人船的水面slam回环检测与位姿优化方法及系统,能够通过实时采集多模态环境感知数据并预处理,构建双层特征库;基于全局轮廓相似度粗匹配筛选候选回环帧,再经局部关键点精匹配及距离、姿态偏差阈值校验,剔除虚假回环并输出约束;同时解耦风浪被动扰动,修正帧间里程计位姿偏差;在分层图优化中依据水面专属异常阈值对约束边权重判别,衰减剔除异常边,迭代求解全局最优位姿;最后增量更新地图并同步双层特征库,保障长航时全局一致性与定位稳定性。

2、一方面,提供了一种适用于无人船的水面slam回环检测与位姿优化方法,该包括:实时采集水面环境的多模态环境感知数据,对预处理后的数据分别提取并整合全局水岸线轮廓特征和局部稳定关键点特征,构建包含全局结构信息和局部细节信息的水面专属双层特征库;基于当前帧的水岸线全局轮廓特征与水面专属双层特征库中的历史全局轮廓特征进行相似度比对,粗匹配筛选出候选回环帧;在筛选出候选回环帧后,对候选回环帧与当前帧的局部稳定关键点特征进行精匹配,并基于回环帧间的距离偏差阈值和姿态角度偏差阈值,剔除异常匹配对及虚假回环匹配对,输出回环约束关系;将无人船姿态分解为主动运动姿态和风浪被动扰动姿态,对高频扰动姿态进行滤波补偿,并修正帧间里程计位姿偏差,分离无人船主动运动位姿与水面风浪被动扰动位姿;在优化过程中,根据水面场景专属异常阈值对回环约束边和里程计约束边进行权重判别,对偏离正常误差区间的异常回环约束和漂移边进行权重衰减并自动剔除,迭代求解得到全局最优六自由度位姿;基于得到的全局最优六自由度位姿,实时更新水面环境地图,对新增环境区域进行实时建图,对历史遍历区域的畸变地图节点和位姿节点进行迭代修正,并同步更新水面专属双层特征库。

3、另一方面,提供了一种适用于无人船的水面slam回环检测与位姿优化系统,该系统包括:水面双层特征库构建模块、全局轮廓粗匹配候选回环帧筛选模块、局部关键点精匹配与回环约束输出模块、帧间里程计位姿预优化模块、异常约束剔除模块和地图增量更新模块;其中,水面双层特征库构建模块,用于实时采集水面环境的多模态环境感知数据,对预处理后的数据分别提取并整合全局水岸线轮廓特征和局部稳定关键点特征,构建包含全局结构信息和局部细节信息的水面专属双层特征库;全局轮廓粗匹配候选回环帧筛选模块,用于基于当前帧的水岸线全局轮廓特征与水面专属双层特征库中的历史全局轮廓特征进行相似度比对,粗匹配筛选出候选回环帧;局部关键点精匹配与回环约束输出模块,用于在筛选出候选回环帧后,对候选回环帧与当前帧的局部稳定关键点特征进行精匹配,并基于回环帧间的距离偏差阈值和姿态角度偏差阈值,剔除异常匹配对及虚假回环匹配对,输出回环约束关系;帧间里程计位姿预优化模块,用于将无人船姿态分解为主动运动姿态和风浪被动扰动姿态,对高频扰动姿态进行滤波补偿,并修正帧间里程计位姿偏差,分离无人船主动运动位姿与水面风浪被动扰动位姿;异常约束剔除模块,用于在优化过程中,根据水面场景专属异常阈值对回环约束边和里程计约束边进行权重判别,对偏离正常误差区间的异常回环约束和漂移边进行权重衰减并自动剔除,迭代求解得到全局最优六自由度位姿;地图增量更新模块,用于基于得到的全局最优六自由度位姿,实时更新水面环境地图,对新增环境区域进行实时建图,对历史遍历区域的畸变地图节点和位姿节点进行迭代修正,并同步更新水面专属双层特征库。

4、有益效果

5、本发明实施例提供的技术方案带来的有益效果至少包括:

6、通过将全局水岸线轮廓特征用于候选帧筛选,并对候选帧的局部稳定关键点进行精匹配和偏差校验,能够使局部外观相似的观测还需满足整体形态及局部几何关系,从而降低错误回环约束被接纳的可能性。

7、通过从时间对齐的数据中估计低频运动分量和高频扰动分量,获得供图优化使用的预优化帧间位姿;将其与回环约束关系共同纳入分层位姿图,对异常残差对应的约束降权和剔除,可减小个别异常约束对局部与全局位姿的牵引,改善地图坐标的一致性。

8、通过按优化后的关键帧位姿修正受影响地图节点,并同步更新水面专属双层特征库,能够使后续检索使用与地图一致的特征位置;对新增区域采用增量更新,能够减少对未变化区域的重复处理。

1.一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于,包括以下步骤:

2.如权利要求1所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:进行相似度比对,粗匹配筛选出候选回环帧的具体流程为:

3.如权利要求2所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:相似度比对包括:对水岸线曲率变化序列进行动态时间规整距离度量或欧氏距离度量,对拐点分布进行匹配度度量,对轮廓跨度和边界起伏趋势进行归一化互相关度量或余弦相似度度量,对水岸线延伸方向和弯曲方向进行角度差度量中的至少一种;将各度量结果按预设权重进行加权融合,得到历史关键帧的全局轮廓相似度得分;

4.如权利要求1所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:输出回环约束关系的具体步骤为:

5.如权利要求4所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:输出回环约束关系,还包括:

6.如权利要求1所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:修正帧间里程计位姿偏差的具体步骤为:

7.如权利要求6所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:修正帧间里程计位姿偏差,还包括:

8.如权利要求1所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:对偏离正常误差区间的异常回环约束和漂移边进行权重衰减并自动剔除的具体步骤为:

9.如权利要求8所述一种适用于无人船的水面slam回环检测与位姿优化方法,其特征在于:对偏离正常误差区间的异常回环约束和漂移边进行权重衰减并自动剔除,还包括:

10.应用如权利要求1-9中任意一项所述一种适用于无人船的水面slam回环检测与位姿优化方法的系统,其特征在于,包括:水面双层特征库构建模块、全局轮廓粗匹配候选回环帧筛选模块、局部关键点精匹配与回环约束输出模块、帧间里程计位姿预优化模块、异常约束剔除模块和地图增量更新模块;