一种多传感器融合的无人船同步定位与建图方法(SLAM)及系统与流程

一种多传感器融合的无人船同步定位与建图方法(slam)及系统
技术领域
1.本发明涉及无人船自主循环技术领域，具体是一种多传感器融合的无人船同步定位与建图方法(slam)及系统。


背景技术：

2.由于水面对光线的反射、波浪的随机扰动、河岸建筑物遮挡以及雾气等对环境传感器的干扰，无人船艇在内河的完全自主巡航是国际性难题。当无人船艇的信号受到干扰或屏蔽时位置和状态信息丢失，美国麻省理工学院wei wang等人研发了“roboat”无人船用于阿姆斯特丹城市水上交通，但是也遇到了桥梁和建筑物阻碍了gps数据的接收，以及阳光直射时激光雷达观察到许多错误的数据等问题。国内有发明专利采用事先测绘桥梁精确位置，穿越前让无人船停泊在桥前固定经纬度、航向角的起始点，再按照固定航向角快速不受控的穿越(202110783834.4)。
3.因此，国内外现有技术难以解决无人船在水面信号受限时的定位与导航，无法完全自主巡航。机器人、无人机和无人车等无人系统常用的同步定位与地图构建(simultaneous localization and mapping,slam)方法解决导航定位和地图更新问题，但是现有slam的传感器对水面场景的复杂环境适应性较差，因此无人船在复杂水面环境自主巡航的关键是解决水面专用的slam技术。


技术实现要素：

4.本发明的目的在于提供一种多传感器融合的无人船同步定位与建图方法(slam)及系统，以解决上述背景技术中提出的问题。
5.为实现上述目的，本发明提供如下技术方案：
6.一种多传感器融合的无人船同步定位与建图方法(slam)，所述同步定位和地图构建的过程包括以下步骤：
7.s1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；
8.s2、对第一点云数据进行特征提取，提取角和面的特征用于slam的关键帧配准；
9.s3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；
10.s4、对第二点云数据进行特征提取，并与所述s2中提取的特征进行融合，形成关键帧；
11.s5、slam系统接收到s4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建。
12.作为本发明进一步的方案：所述slam采用因子图优化方法，给定传感器测量值的船舶状态所述slam系统因子包括激光雷达里程计因子、gps因子、回环因子；
13.其中：d
x
和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角。
14.作为本发明进一步的方案：所述因子使用启发式方法添加新节点当船舶的位置或旋转变化超过用户定义的阈值时，添加新节点x，使用贝叶斯树的增量平滑和映射来优化插入新节点后的因子图。
15.作为本发明进一步的方案：所述激光雷达里程计因子的构建包括以下步骤：
16.t1、在对固态激光雷达的原始点云进行特征提取的基础上，进一步提取点云图像的水面和岸边或其他水面固体物质的边缘特征，作为点云帧的选择标准，再进行帧间特征匹配流程；
17.t2、帧间特征配准算法是通过匹配激光扫描数据中两次相邻帧的特征，建立以欧式距离等为度量标准的目标函数，并求解以两次扫描点云之间的变换关系为优化变量的最小二乘问题，即平移和旋转变换，进而求得在两帧的间隔时间内移动机器人的局部位姿变换，进而求得在两帧t与t+1的间隔时间内无人船的局部位姿变换t
(t,t+1)
；
18.t3、对于t时刻和t+1时刻获得的相邻两帧激光雷达点云f
t
、f
t+1
，设在其中提取的特征角点集和特征平面点集分别为f
tc
、f
tp
，在时间t提取的所有特征组成一个激光雷达点云帧f
t
，其中f
t
＝{f
tc
,f
tp
}；
19.t4、当船只的姿态变化超过用户定义的阈值时，选择一个激光雷达帧f
t+1
作为关键帧。选择的规则是当船舶的位置变化或旋转角度变化超过一定的阈值；
20.t5、两个关键帧之间的激光雷达帧将被丢弃，当选择一个激光雷达关键帧时，采用扫描匹配来计算新关键帧与先前子关键帧之间的相对变换，子关键帧是通过将前n个关键帧转换为全局帧来获得的，然后将这些变换的关键帧合并为像素地图m
t
，m
t
由两个子像素图组成，即上一步中提取的点和面两种类型的特征，这两个子像素图分别表示为(点特征像素图)和(面特征像素图)，特征集和像素图之间的关系可以表示为：
[0021][0022]
其中：
[0023][0024]
其中和是全局框架中变换的点和平面特征，新的激光雷达关键帧f
t+1
使用imu的初始猜测转换为全局帧，全局帧中变换后的新关键帧表示为'f
t+1
，针对上述关键帧及像素集，使用最近邻搜索的方法进行扫描匹配，并获得他们之间的相对变换δt
t,t+1
，作为激光雷达里程计因子添加到因子图中。
[0025]
作为本发明进一步的方案：所述gps组合导航因子构建方法，在slam系统中，无人船利用gps、北斗和惯性导航组合模块的绝对测量值，组合导航的位姿信息源通过时间戳，融入至激光雷达里程计的点云帧中，并将其作为gps因子纳入因子图中。
[0026]
作为本发明进一步的方案：所述回环因子在slam系统中的回环检测用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，所述回环因子关键帧匹配包括以下步骤：
[0027]
l1、输入当前时刻的特征点云；
[0028]
l2、在前一时刻的特征点云中搜索当前时刻特征点云的最邻近点和次邻近点；
[0029]
l3、计算当前时刻特征点云中的点到近邻边缘线的距离；
[0030]
l4、堆叠距离约束，并通过l-m算法迭代求解相对变化；
[0031]
l5、获取迭代次数并判断迭代次数是否等于设定值且迭代次数小于最大值，若是则通过相对变化对当前时间的特征点云进行变化并执行步骤l1，若否则执行步骤l6；
[0032]
l6、判断迭代次数是否等于最大值且距离均小于阈值，若否则执行步骤l4，若是则执行步骤l7；
[0033]
l7、输出相对变化解。
[0034]
作为本发明进一步的方案：所述slam地图构成是通过获取每个关键帧的准确姿势和每个关键帧对应的三维点云的位姿，通过坐标变化，将激光三维点云坐标有每时刻的激光坐标系转换到全局的地图坐标并拼接生产三维地图。
[0035]
作为本发明进一步的方案：所述同步定位和地图构建后，在卫星信号正常时，无人船使用卫星和slam组合导航，并对无人船进行定位以及地图更新，在卫星信号缺少时，无法使用卫星导航时，自动切换至惯性导航模式并计算无人船位姿信息，作为slam的位姿校准和时间戳同步，通过slam系统进行导航
[0036]
一种多传感器的无人船同步定位与建图系统，包括无人船，所述无人船上设有slam系统，所述无人船上设有与slam系统信号连通的固态激光雷达、低线束全景机械激光雷达、卫星和惯性导航模块。
[0037]
与现有技术相比，本发明的有益效果是：本发明基于slam算法，对slam系统合伙算法进行深度的优化和改进，使得本技术的无人船同步定位和地图构建的方法，可以适应复杂的水面环境，解决常规视觉和激光雷达融合传感器对光线干扰敏感和配准误差较大问题，使得无人船能够在复杂的水面环境上自动探测和识别障碍物及桥梁等目标物，进行自主航向，而且在没有卫星信号的情况下，通过slam技术仍可以安全自主航向。
附图说明
[0038]
图1为本实施例因子图优化的内河无人船舶slam原理示意图；
[0039]
图2、图3为本实施例水面桥梁的关键特征位姿转换示意；
[0040]
图4为本实施例关键帧匹配算法流程示意图；
[0041]
图5为本实施例三维点云地图拼接示意图。
具体实施方式
[0042]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0043]
请参阅图1-5，本发明实施例中，无人船同步定位与建图系统，包括无人船，无人船上设有slam系统，无人船上设有与slam系统信号连通的固态激光雷达、低线束全景机械激光雷达、卫星和惯性导航模块，所述固态激光雷达用于探测无人船前方一定视角范围(例如120度)的水面环境，并生成点云数据，进一步对点云图像进行识别，识别水面目标的类型，如河岸、桥梁、浮标、船舶或其他障碍物，识别的结果作为无人船巡航场景自动切换，如穿越桥梁、障碍规，所述机械激光雷达用于探测无人船周边360度的全景水面环境，并生成点云数据
[0044]
一种多传感器融合的无人船同步定位与建图方法(slam)，基于slam方法进行同步定位和地图构建，slam(simultaneous localization and mapping)是机器人、无人机和无人车等无人系统常用的同步定位与地图构建，本实施例中的同步定位和地图构建基于slam系统，其过程包括以下步骤：
[0045]
s1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；
[0046]
s2、对第一点云数据进行特征提取，提取角和面的特征用于slam的关键帧配准；
[0047]
s3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；
[0048]
s4、对第二点云数据进行特征提取，并与s2中提取的特征进行融合，融合后的数据同步时间戳，形成关键帧，作为关键帧用于slam系统中的激光雷达里程计和回环检测配准，所述点云数据同时计算无人船两边和后部的河岸或者障碍物位置和距离，用于障碍规避或河岸靠边巡航；
[0049]
s5、slam系统接收到s4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建。
[0050]
slam由固态激光雷达、机械激光雷达、组合导航传感器组成，采用因子图优化方法。因子图优化slam方法指在估计给定传感器测量值的船舶状态d
x
和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角，为了无缝地结合来自各种传感器的测量，本发明利用来建模这个问题，slam系统采用因子图建模方法，slam系统的三类因子是激光雷达里程计因子、gps因子以及回环因子，因子是为了限制内存使用并提高定位系统的效率，使用启发式方法向图中添加新节点仅当船舶的位置或旋转变化超过用户定义的阈值时，才会添加新节点x，再使用贝叶斯树的增量平滑和映射来优化插入新节点后的因子图。
[0051]
激光雷达里程计因子的构建包括以下步骤：
[0052]
t1、在对固态激光雷达的原始点云进行特征提取的基础上，进一步提取点云图像的水面和岸边或其他水面固体物质的边缘特征，作为点云帧的选择标准，再进行帧间特征匹配流程；
[0053]
t2、帧间特征配准算法是通过匹配激光扫描数据中两次相邻帧的特征，建立以欧式距离等为度量标准的目标函数，并求解以两次扫描点云之间的变换关系为优化变量的最小二乘问题，即平移和旋转变换，进而求得在两帧的间隔时间内移动机器人的局部位姿变换，在本实施例中，如图2所示为水面桥梁的激光雷达点云图像及关键特征，图3为角和面随时间的位姿变换关系，圆点表示特征角点，三角点表示特征平面点，桥梁上的箭头表示匹配关系，桥梁下的箭头表示t时刻到t+1时刻待求解的位姿变换t
(t,t+1)
，进而求得在两帧t与t+1的间隔时间内无人船的局部位姿变换t
(t,t+1)
；
[0054]
t3、对于t时刻和t+1时刻获得的相邻两帧激光雷达点云f
t
、f
t+1
，设在其中提取的特征角点集和特征平面点集分别为f
tc
、f
tp
，在时间t提取的所有特征组成一个激光雷达点云帧f
t
，其中f
t
＝{f
tc
,f
tp
}；
[0055]
t4、为了提供计算的效率和速度，slam通常采用了关键帧选择的概念，这在视觉slam领域中得到了广泛的应用，当船只的姿态变化超过用户定义的阈值时，选择一个激光雷达帧f
t+1
作为关键帧。选择的规则是当船舶的位置变化或旋转角度变化超过一定的阈值；
[0056]
t5、两个关键帧之间的激光雷达帧将被丢弃，当选择一个激光雷达关键帧时，采用
扫描匹配来计算新关键帧与先前子关键帧之间的相对变换，子关键帧是通过将前n个关键帧转换为全局帧来获得的，然后将这些变换的关键帧合并为像素地图m
t
，m
t
由两个子像素图组成，即上一步中提取的点和面两种类型的特征，这两个子像素图分别表示为(点特征像素图)和(面特征像素图)，特征集和像素图之间的关系可以表示为：
[0057][0058]
其中：
[0059][0060]
其中f
tc
和'f
tp
是全局框架中变换的点和平面特征，新的激光雷达关键帧f
t+1
使用imu的初始猜测转换为全局帧，全局帧中变换后的新关键帧表示为'f
t+1
，针对上述关键帧及像素集，使用最近邻搜索的方法进行扫描匹配，并获得他们之间的相对变换δt
t,t+1
，作为激光雷达里程计因子添加到因子图中。
[0061]
所述gps组合导航因子构建方法，在slam系统中，无人船可以通过使用激光雷达里程计因子来实现低漂移状态估计，但定位系统在长时间的导航任务中仍然存在漂移问题。因此，利用gps、北斗和惯性导航组合模块的绝对测量值，组合导航的位姿信息源通过时间戳，融入至激光雷达里程计的点云帧中，并将其作为gps因子纳入因子图中。
[0062]
所述回环因子构建方法，slam主要是针对没有先验信息的未知环境进行定位与建图，现有的slam系统具备对已形成地图的历史场景进行判断和识别的能力，通常也被称为回环闭合检测，由于无人船在内河的作业通常是顺序执行不重复路径，因此可理解为“无穷的走廊”，本技术称为顺序slam，在该系统中的回环检测主要用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，具体算法原理如图4所示，回环因子在slam系统中的回环检测用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，回环因子关键帧匹配包括以下步骤：
[0063]
l1、输入当前时刻的特征点云；
[0064]
l2、在前一时刻的特征点云中搜索当前时刻特征点云的最邻近点和次邻近点；
[0065]
l3、计算当前时刻特征点云中的点到近邻边缘线的距离；
[0066]
l4、堆叠距离约束，并通过l-m算法迭代求解相对变化；
[0067]
l5、获取迭代次数并判断迭代次数是否等于设定值且迭代次数小于最大值，若是则通过相对变化对当前时间的特征点云进行变化并执行步骤l1，若否则执行步骤l6；
[0068]
l6、判断迭代次数是否等于最大值且距离均小于阈值，若否则执行步骤l4，若是则执行步骤l7；
[0069]
l7、输出相对变化解。
[0070]
由于使用了因子图，回环检测可以无缝地整合到slam系统中，因子图主要是通过帧间配准得到的实时位姿估计结果和观测约束条件建立，因子图由点和边构成，其中点表示无人船不同时刻的位姿，边表示不同时刻位姿之间的约束因子，基于欧几里德距离的简单但有效的闭环检测方法。
[0071]
当新的状态x
i+1
被添加到因子图中时，如图1所示，搜索图并找到距离x
i+1
一定距离内的先验状态。例如，x3是返回的候选状态，提取子关键帧{f
3-m
，
…
，f3，
…
，f
3+m
}，并将它们合
并到一个局部像素图中，如果可以在f
i+1
和像素图之间找到成功的匹配，就可以得到相对变换δt
3,i+1
，将其作为循环闭合因子添加到图表中。
[0072]
基于slam的导航地图的拼接，经过上述图优化的过程，每个关键帧的姿态都是最优的，已知关键帧的准确位姿和每个关键帧对应的三维点云的位姿，通过坐标变换，将激三维光点云坐标由每时刻的激光坐标系，转换到全局的地图坐标系，即完成了三维地图的拼接。船载固态和机械激光雷达的坐标系标定，标定的目的是以正向安装的固态激光雷达坐标系为基准，将机械激光雷达的点云转换到该坐标系下。坐标转换关系可以用旋转矩阵r和平移向量t表示，通过levenberg-marquart算法最终计算出r和t两个坐标系之间的旋转矩阵和平移向量。
[0073]
组合导航在卫星信号正常时用于无人船的导航、定位及地图更新，在卫星信号缺少时，自动切换至惯性导航模式并计算无人船位姿信息，作为slam的位姿校准和时间戳同步
[0074]
一种多传感器的无人船组合导航方法，在卫星信号正常时，无人船使用卫星和slam组合导航，并对无人船进行定位以及地图更新，在卫星信号缺少时，无法使用卫星导航时，自动切换至惯性导航模式并计算无人船位姿信息，作为slam的位姿校准和时间戳同步，通过slam系统进行导航。
[0075]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
[0076]
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。