一种基于多传感器融合的河道环境同步定位与地图构建方法及系统与流程

本发明涉及水面测绘与导航定位，尤其涉及一种基于多传感器融合的河道环境同步定位与地图构建方法及系统。

背景技术：

1、河道环境同步定位与地图构建是水域测绘、航道监测和无人船导航的重要基础。现有技术通常在水面或水下载体上搭载gnss、惯性测量单元、视觉传感器、激光雷达和声呐等设备，以获取河道环境中的位置、姿态、视觉和声学等信息。河道环境一般具有岸线弯曲、宽度变化、桥梁与植被遮挡、水面反射及多路径效应显著等特点。在此类环境中，各类传感器分别以不同采样频率和工作方式获取数据，并用于载体的导航与环境感知。

2、然而在河道岸线弯曲、桥梁植被遮挡及水面反射显著等复杂条件下，gnss信号因遮挡和反射导致定位不连续，惯性测量单元因积分运算导致航向误差随时间累积，视觉与激光雷达因水面波动和遮挡产生特征误提取，声呐测深因载体姿态变化和换能器偏差产生波束指向误差；由于多源传感器在时间基准、空间基准和安装位置方面存在差异，数据在统一融合时出现时空错位，岸线信息与水下地形信息难以准确关联，导致地图层次孤立、融合时相互矛盾；上述误差在复杂河道环境中随时间累积并相互传播，导致实时定位出现横向偏移和航向偏差，在gnss拒止或水深平坦等退化场景下定位鲁棒性不足。

技术实现思路

1、本发明提供一种基于多传感器融合的河道环境同步定位与地图构建方法及系统，其主要目的在于解决河道复杂环境下，多源传感器数据难以统一融合、载体惯性航向易漂移、岸线信息与水下地形信息未有效关联、地图层次孤立、实时定位鲁棒性不足的问题。

2、为实现上述目的，本发明提供一种基于多传感器融合的河道环境同步定位与地图构建方法，包括：

3、s1：利用搭载多源传感器的载体，采集河道环境中的多源传感器数据，并对所述多源传感器数据进行时间同步和空间配准，生成传感器融合数据；

4、s2：从所述传感器融合数据中提取河道岸线轮廓特征，生成岸线轮廓点序列，从所述传感器融合数据中提取惯性测量数据；

5、s3：基于所述岸线轮廓点序列识别河道两岸的岸线走向及岸线间距变化趋势，生成岸线几何约束关系，根据所述惯性测量数据确定所述载体的运动方向，当所述运动方向偏离所述岸线走向时，依据所述岸线几何约束关系对所述运动方向进行校正，生成载体校正位姿信息；

6、s4：利用所述传感器融合数据中的声呐测深数据，获取河道底部各位置的水深值，根据所述水深值的空间分布确定所述河道底部的地形起伏特征，将所述地形起伏特征与所述载体校正位姿信息进行关联，生成河道底部地形图；

7、s5：将所述岸线轮廓点序列构建为岸线层地图，将所述河道底部地形图构建为水深层地图，基于所述载体校正位姿信息确定所述岸线层地图与所述水深层地图之间的空间对应关系，进行空间关联融合，生成所述河道环境的多层次融合地图；

8、s6：基于所述河道环境的多层次融合地图，对所述载体的位置进行实时定位更新，生成实时位置信息。

9、优选的，所述对所述多源传感器数据进行时间同步和空间配准，生成传感器融合数据，包括：

10、以所述多源传感器数据中的采集时刻和航向数据标定河道横向断面，将所述多源传感器数据归并至所述河道横向断面，得到时间同步数据；

11、依据所述时间同步数据，将所述多源传感器数据中的声呐测深数据归正至所述多源传感器数据中的水面高程，得到垂向位置归属；

12、在所述垂向位置归属下，将所述多源传感器数据中水面跨距与测深跨距的两端相对重合，得到横向基准；

13、在所述垂向位置归属和所述横向基准之下，对所述多源传感器数据中的各路观测数据进行空间配准，得到所述传感器融合数据。

14、优选的，所述基于所述岸线轮廓点序列识别河道两岸的岸线走向及岸线间距变化趋势，生成岸线几何约束关系，包括：

15、基于所述岸线轮廓点序列中相邻的岸线轮廓点，识别出河道两岸的岸线走向；

16、结合所述河道两岸的岸线走向，比对所述岸线轮廓点序列中配对岸线轮廓点的相隔长度，识别出所述河道两岸的岸线间距变化趋势；

17、以所述河道两岸的岸线间距变化趋势印证所述河道两岸的岸线走向，得到更正岸线走向；

18、剔除所述岸线轮廓点序列中与所述更正岸线走向不相符的岸线轮廓点，得到相符岸线轮廓点；

19、以所述相符岸线轮廓点和所述岸线间距变化趋势约束所述河道两岸，生成岸线几何约束关系。

20、优选的，所述根据所述惯性测量数据确定所述载体的运动方向，当所述运动方向偏离所述岸线走向时，依据所述岸线几何约束关系对所述运动方向进行校正，生成载体校正位姿信息，包括：

21、根据所述惯性测量数据中的航向角与所述岸线几何约束关系，确定所述载体的运动方向；

22、将所述运动方向与所述岸线走向相互比对，得到所述运动方向相对所述岸线走向的航向偏离量；

23、沿所述岸线间距变化趋势核对所述航向偏离量与所述岸线走向的转折，标记其中随岸线转向的河道转向偏离量；

24、排除所述河道转向偏离量后，依据所述岸线几何约束关系，甄别所述航向偏离量中偏离岸线走向的航向漂移偏离量；

25、依据所述航向漂移偏离量与所述河道转向偏离量，对所述运动方向予以校正，生成载体校正位姿信息。

26、优选的，所述利用所述传感器融合数据中的声呐测深数据，获取河道底部各位置的水深值，包括：

27、利用所述传感器融合数据中的声呐测深数据，提取各波束的波束斜距值；

28、根据所述声呐测深数据中的波束发射角度与所述载体校正位姿信息中的横滚角及俯仰角，还原各波束的实际指向；

29、以所述载体校正位姿信息所确定的声呐换能器位置为起点，沿所述实际指向延伸所述波束斜距值，得到所述河道底部的回波点；

30、依据所述岸线几何约束关系中的岸线走向，自所述岸线轮廓点序列中选取位于河岸与水面交界处的岸线点，得到水涯线点；

31、以所述水涯线点的竖直高度为起算面，获取所述回波点处所述河道底部各位置的水深值。

32、优选的，所述根据所述水深值的空间分布确定所述河道底部的地形起伏特征，将所述地形起伏特征与所述载体校正位姿信息进行关联，生成河道底部地形图，包括：

33、按所述声呐测深数据中波束脚印的已知间距对所述水深值的空间分布进行相邻比对，得到河道底部的地形突变界线；

34、以所述地形突变界线与所述岸线走向的夹角，对所述河道底部进行走向归类，得到所述河道底部的地形起伏特征；

35、以所述地形起伏特征的深槽走向为基准，对所述载体校正位姿信息进行偏差比对，得到所述载体校正位姿信息的位姿偏差；

36、按所述位姿偏差对所述载体校正位姿信息进行反向归正，得到所述地形起伏特征与所述载体校正位姿信息关联的吻合位姿；

37、以所述吻合位姿对所述水深值的空间分布进行依序铺展，得到所述河道底部的图幅边界与所述岸线轮廓点序列重合的河道底部地形图。

38、优选的，所述将所述岸线轮廓点序列构建为岸线层地图，将所述河道底部地形图构建为水深层地图，包括：

39、按所述载体校正位姿信息所指示的载体朝向，对所述岸线轮廓点序列进行分列，得到所述岸线轮廓点序列的左岸岸线要素与右岸岸线要素；

40、沿所述载体校正位姿信息所指示的行进方向，对所述左岸岸线要素与所述右岸岸线要素逐处赋予编号，并依序组合所述左岸岸线要素与所述右岸岸线要素，得到岸线层地图；

41、自所述左岸岸线要素与所述右岸岸线要素起，将所述河道底部地形图临岸处的所述水深值归零，得到岸坡地段；

42、沿所述岸坡地段与所述水深值，对所述河道底部地形图进行深槽定界，得到水深层地图。

43、优选的，所述基于所述载体校正位姿信息确定所述岸线层地图与所述水深层地图之间的空间对应关系，进行空间关联融合，生成河道环境的多层次融合地图，包括：

44、基于所述载体校正位姿信息，将所述岸线层地图与所述水深层地图逐位配对，确定二者之间的空间对应关系；

45、依据所述空间对应关系与所述水深值，在所述水深层地图中辨识临水边界；

46、将所述临水边界与所述岸线轮廓点序列相互核对，得出所述空间对应关系中的岸线水深关联区；

47、基于所述岸线水深关联区，将所述岸线层地图与所述水深层地图，进行空间关联融合，得到空间关联融合结果；

48、依据所述空间关联融合结果与所述岸线几何约束关系，取舍所述岸线轮廓点序列，生成所述河道环境的多层次融合地图。

49、优选的，所述基于所述河道环境的多层次融合地图，对所述载体的位置进行实时定位更新，生成实时位置信息，包括：

50、将所述岸线轮廓点序列投影至所述岸线层地图，并与左右岸线点序列沿岸线走向比对，得到所述载体的位置中的横向偏移量；

51、依据所述横向偏移量与所述岸线间距变化趋势逐段比对两岸线间距，得到所述岸线层地图中的等宽区段；

52、在所述等宽区段内比对所述河道底部地形图与所述水深层地图的水深分布，得到所述水深层地图中的沿岸向偏移量；

53、以所述运动方向和所述岸线几何约束关系校验所述沿岸向偏移量，得到所述载体的位置更新结果；

54、基于所述多层次融合地图和所述位置更新结果，对所述载体的位置进行实时定位更新，生成实时位置信息。

55、为了解决上述问题，本发明还提供了一种基于多传感器融合的河道环境同步定位与地图构建系统，包括：

56、同步配准模块：利用搭载多源传感器的载体，采集河道环境中的多源传感器数据，并对所述多源传感器数据进行时间同步和空间配准，生成传感器融合数据；

57、特征提取模块：从所述传感器融合数据中提取河道岸线轮廓特征，生成岸线轮廓点序列，从所述传感器融合数据中提取惯性测量数据；

58、位姿校正模块：基于所述岸线轮廓点序列识别河道两岸的岸线走向及岸线间距变化趋势，生成岸线几何约束关系，根据所述惯性测量数据确定所述载体的运动方向，当所述运动方向偏离所述岸线走向时，依据所述岸线几何约束关系对所述运动方向进行校正，生成载体校正位姿信息；

59、地形构建模块：利用所述传感器融合数据中的声呐测深数据，获取河道底部各位置的水深值，根据所述水深值的空间分布确定所述河道底部的地形起伏特征，将所述地形起伏特征与所述载体校正位姿信息进行关联，生成河道底部地形图；

60、地图融合模块：将所述岸线轮廓点序列构建为岸线层地图，将所述河道底部地形图构建为水深层地图，基于所述载体校正位姿信息确定所述岸线层地图与所述水深层地图之间的空间对应关系，进行空间关联融合，生成所述河道环境的多层次融合地图；

61、定位更新模块：基于所述河道环境的多层次融合地图，对所述载体的位置进行实时定位更新，生成实时位置信息。

62、有益效果

63、本方案通过采集河道多源传感器数据并进行时间同步和空间配准，从融合数据中提取岸线轮廓点序列和惯性测量数据，识别河道两岸岸线走向及岸线间距变化趋势并生成岸线几何约束，再结合惯性测量数据对载体运动方向进行校正等手段，做到了把不同传感器数据统一到同一时间和空间基准下，并把岸线走向和河宽变化作为校正航向的参考，实现了在岸线弯曲、桥梁植被遮挡、水面反射等复杂条件下仍能获得较准的载体位姿，解决了现有技术中多源传感器数据难以统一融合、载体惯性航向易漂移、岸线信息与水下地形信息未有效关联的问题，具有提高多源数据融合精度、抑制航向误差累积、增强复杂河道环境适应性的好处。

64、本方案通过声呐测深数据获取河道底部各位置水深并生成河道底部地形图，把岸线轮廓点序列构建为岸线层地图、把河道底部地形图构建为水深层地图，再依据校正位姿确定两者空间对应关系并进行关联融合，生成多层次融合地图，并基于该地图对载体位置进行实时定位更新等手段，做到了岸线层与水深层在同一空间基准下相互对应、地图层次不再孤立，实现了岸线边界、水深分布和载体位置之间的相互校验与持续修正，解决了现有技术中地图层次孤立、融合时相互矛盾、实时定位鲁棒性不足的问题，具有地图表达更完整、定位结果更稳定、在gnss拒止或水底地形平坦退化场景下仍能保持较好定位效果的好处。同时，利用校正位姿还原声呐波束实际指向，减少载体姿态变化和换能器安装偏差对水深测量的影响。