一种河道船舶行驶定位导航的方法及系统与流程

本发明涉及一种河道船舶行驶定位导航的方法及系统，属于船舶无人驾驶技术领域。

背景技术：

在过去的几十年里,河道船舶自主导航在定位、目标检测、路径规划和跟踪控制等方面都取得了很大的进展。然而，目前的水下无人器通常是为开阔水域开发的，在狭窄和拥挤的城市水环境中这些方式的控制效果都会大打折扣。现今船舶通常使用全球定位系统和惯性测量单元(由扩展卡尔曼滤波器(ekf)或无迹卡尔曼滤波器(ukf)融合)来实现定位，这通常会产生米级精度。这些基于全球定位系统惯性测量组合的方法在城市水道中可能不稳定，在那里全球定位系统信号通常会严重衰减。那么基于这种现状，无人船舶在狭窄水道中的航行会比在开阔水域中更加具有挑战性。

技术实现要素：

本发明的目的在于提供一种河道船舶行驶定位导航的方法，以解决现有技术缺陷。

一种河道船舶同时定位与建立地图的方法，所述方法包括步骤：

前端里程计通过传感器组实时发送过来的数据，并通过相邻的点云数据、图像信息估算出传感器组在该时刻的位姿信息数据；

将前端里程计传过来的位姿信息数据发送到预先构建的后端，得到全局一致的轨迹和地图。

进一步地，所述位姿信息数据使用ndt算法实现点云的匹配。

进一步地，所述后端模型采用g2o的图优化位姿信息数据。

进一步地，所述点云数据采用正态分布变换匹配算法处理测量噪声。

进一步地，所述方法还包括：船舶开始启动以后，开始获取传感器信息，并对传感器获取的信息进行预处理。

进一步地，所述方法还包括：

通过回环检测通过传感器采集数据的当前帧和历史数据的关键帧进行匹配，将匹配后的数据传送至后端。

一种河道船舶同时定位与建立地图的系统，所述系统包括：

3d激光雷达：用于定位和障碍物检测；

rgb-d相机：用于为扩展卡尔曼滤波器定位提供视觉里程计；

惯性测量单元：用于监控机器人的姿态、线性加速度和角速度；

主控制器：用于接收3d激光雷达、rgb-d相机和惯性测量单元发生的数据并输出船舶的实时位姿；

推进装置：根据船舶的实时位姿驱动船舶运行。

与现有技术相比，本发明所达到的有益效果：本方法在船舶上安装一些船载传感器，可以实现狭窄河道上的船舶自主定位与构图，方案所需硬件尺寸小、质轻，应用便捷，而且平台简单易懂，适合后续改进后的扩展。

附图说明

图1是船舶示意图；

图2是同时定位与建图流程图；

图3是实现船舶定位方法示意图；

图4是船舶动力推进流程图；

图中：1-3d激光雷达、2-支架、3-rgb-d相机、4-惯性测量单元。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

如图1-图4所示，公开了一种河道船舶同时定位与建立地图的方法；所述方法为以下几个步骤：

步骤一：船舶开始启动以后，开始获取传感器组信息，并对传感器组信息进行预处理；

步骤二：前端里程计利用传感器组实时发送过来的数据，通过相邻的点云数据、图像信息等来估算出传感器组在该时刻的位姿信息，其中我们使用ndt算法实现点云的匹配，与惯性测量单元4融合以实现减小累计误差；

步骤三：考虑到前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，随时间累积将会导致地图失准。所以，在此基础上，我们构建一个尺度、规模更大的优化问题即后端优化，后端在接收到前端里程计传过来的位姿估计数据以后，采用g2o的图优化以得到全局一致的轨迹和地图；

步骤四：为了避免累积误差影响到长时间轨迹和地图估计的正确性，引入回环检测，使得当前数据和历史数据产生关联，必要时可以利用回环检测进行重定位。在回环检测中利用3d激光雷达1的当前帧和以往的关键帧进行匹配；进而提升整个slam系统的准确性和鲁棒性。

步骤五：为了后续的避障和导航，船舶基于上述的经过处理以后的数据信息，进行稠密地图的建立。

第二方面：本发明还公开了一种河道船舶同时定位与建立地图的系统，所述系统包括：包含3d激光雷达1，惯性测量单元4，rgb-d相机3组成了传感器组；支架2横放于船体正中，所述3d激光雷1达作为主传感器安装在支架2的顶部中心，用于定位和障碍物检测；一个惯性测量单元4设在平行于船舶主体的主轴处，以监控机器人的姿态、线性加速度和角速度。

3d激光雷达1下方即支架2下侧安装了一个rgb-d相机3，为ekf定位提供视觉里程计。

所述的信号传输流程图，各个传感器获取的数据输入到主控制器xavier中slam算法中相应的输入接口，经过算法处理输出了船舶的实时位姿，并对下一时刻的轨迹进行预测。xiaver相关的输出信号输入给单片机stm32，由单片机stm32输出对应的动力控制信号(线速度、角速度)给推进装置，实现船舶的运行。

所述定位实现船舶定位示意图，传感器获得外部信息以后，我们将传感器测得的数据进行扩展卡尔曼(ekf)滤波，对于雷达测得的点云数据，我们采用正态分布变换(ndt)匹配算法来处理测量噪声，更具体地说，我们将地图表示为3d网格，并为每个网格点分配一个概率分布。这样，无损检测匹配算法将检测到的点与地图上的分布相匹配。考虑到惯导里程计的累计误差，我们再利用rgb-d测得的图像进行视觉slam进行扩展卡尔曼滤波的额外测量，通过上述方式实现船舶自身的精确定位。

最终，为了后续的避障和导航，船舶基于上述的经过处理以后的数据信息，进行稠密地图的建立。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

技术特征：

1.一种河道船舶同时定位与建立地图的方法，其特征在于，所述方法包括步骤：

前端里程计通过传感器组实时发送过来的数据，并通过相邻的点云数据、图像信息估算出传感器组在该时刻的位姿信息数据；

将前端里程计传过来的位姿信息数据发送到预先构建的后端，得到全局一致的轨迹和地图。

2.根据权利要求1所述的河道船舶行驶定位导航的方法，其特征在于，所述位姿信息数据使用ndt算法实现点云的匹配。

3.根据权利要求1所述的河道船舶行驶定位导航的方法，其特征在于，所述后端模型采用g2o的图优化位姿信息数据。

4.根据权利要求1所述的河道船舶行驶定位导航的方法，其特征在于，所述点云数据采用正态分布变换匹配算法处理测量噪声。

5.根据权利要求1所述的河道船舶行驶定位导航的方法，其特征在于，所述方法还包括：船舶开始启动以后，开始获取传感器信息，并对传感器获取的信息进行预处理。

6.根据权利要求1所述的河道船舶行驶定位导航的方法，其特征在于，所述方法还包括：

通过回环检测通过传感器采集数据的当前帧和历史数据的关键帧进行匹配，将匹配后的数据传送至后端。

7.一种河道船舶同时定位与建立地图的系统，其特征在于，所述系统包括：

3d激光雷达：用于定位和障碍物检测；

rgb-d相机：用于为扩展卡尔曼滤波器定位提供视觉里程计；

惯性测量单元：用于监控机器人的姿态、线性加速度和角速度；

主控制器：用于接收3d激光雷达、rgb-d相机和惯性测量单元发生的数据并输出船舶的实时位姿；

推进装置：根据船舶的实时位姿驱动船舶运行。

技术总结
本发明公开了一种河道船舶同时定位与建立地图的方法，所述方法包括步骤：前端里程计通过传感器组实时发送过来的数据，并通过相邻的点云数据、图像信息估算出传感器组在该时刻的位姿信息数据；将前端里程计传过来的位姿信息数据发送到预先构建的后端，得到全局一致的轨迹和地图；本方法在船舶上安装一些船载传感器，可以实现狭窄河道上的船舶自主定位与构图，方案所需硬件尺寸小、质轻，应用便捷，而且平台简单易懂，适合后续改进后的扩展。

技术研发人员：吴炜;付勇
受保护的技术使用者：苏州市卫航智能技术有限公司
技术研发日：2021.03.30
技术公布日：2021.08.06