一种基于SLAM技术的船舶进出港三维场景重建与展示系统

摘要

本发明公开了一种基于SLAM技术的船舶进出港三维场景重建与展示系统，包括基于SLAM技术实现港口三维地图和船舶模型构建、船舶GPS和IMU数据通过无线网络通信与船舶进出港三维展示平台进行数据交互以及船舶进出港三维展示平台构建，港口三维地图与船舶三维模型的构建用于在船舶进出港三维展示平台展现港口周围环境信息和进出港的船舶形态，船舶进出港时位置和姿态的动态更新用于在船舶进出港三维展示平台实时监测船舶进出港的运动状态和方位信息，船舶进出港三维展示平台用于进行人机交互、地图和模型的加载显示、实时动态更新船舶的位置和姿态以及港口与船舶相关信息的显示。可移植性好、分辨率高以及可视化性能好。

说明书

技术领域

本发明涉及船舶智能化技术领域，特别是涉及一种基于SLAM技术的船舶进出港三维场景重建与展示系统。

背景技术

船舶作为一种海洋利器，其广泛应用在货物运输、旅游观光、海鲜捕捞和海上作战等领域，全球船舶的需求量正在逐年增加，2021年中国稳坐世界第一造船大国。大量的船舶需求推动着航运经济的快速发展，而港口作为船舶的停靠点，对航运经济有着重大影响，其智能化水平，将直接影响船舶货物装卸的效率，船舶进出港作为智能港口的重要环节，提升其高效性、直观性以及智能化水平至关重要。

当前，船舶进出港口主要依靠引航员来完成，引航员根据船舶运动与控制的基本原理，结合本船性能、特点以及当时环境信息,正确使用车、舵、锚、缆等工具及时修正航向和流压,并充分发挥拖轮功能实现船舶的安全进出港。然而，岸基人员却无法实时直观细致掌握船舶进出港状态，只能通过船舶自动识别系统(AIS)了解船舶周围状态，船舶自动识别系统，其是指一种应用于船和岸基、船和船之间的海事安全与通信的新型助航系统。常由VHF通信机、GPS定位仪和与船载显示器及传感器等相连接的通信控制器组成，能自动交换船位、航速、航向、船名、呼号等重要信息。装在船上的AIS在向外发送这些信息的同时，同样接收VHF覆盖范围内其他船舶的信息，从而实现自动应答。此外，作为一种开放式数据传输系统,它可与雷达、ARPA、ECDIS、VTS等终端设备和INTERNET实现连接，构成海上交管和监视网络，不使用雷达探测也能获取交通信息，可以有效减少船舶碰撞事故。AIS的工作原理决定其受外界环境干扰因素较多，且可视化性能较差。此外，进行港口三维地图和船舶三维模型的动画制作，因其可视化效果好，正在成为一种新的解决方式，但动画制作成本高昂、绝对地理信息需要手动标定、且我国港口众多可移植性较差，无法真实简便的展示船舶进出港。

综上，目前用于船舶进出港监测和三维实时展示的方法包括：基于船舶自动识别系统(AIS)监测船舶进出港状态、采用动画制作方式实时展示船舶进出港状态等。但已有方法存在可视性效果差或通用性差等特点，无法简单、真实、形象的展示船舶进出港。

发明内容

针对以上技术问题，本发明提供一种基于SLAM技术的船舶进出港三维场景重建与展示系统。

本发明解决其技术问题采用的技术方案是：

一种基于SLAM技术的船舶进出港三维场景重建与展示系统，包括港口三维地图与船舶三维模型构建模块、船舶进出港时位置和姿态的动态更新模块和船舶进出港三维展示平台，船舶进出港时位置和姿态的动态更新模块通过无线通信方式连接船舶进出港三维展示平台，

港口三维地图与船舶三维模型构建模块用于获取多传感器采集到的激光雷达点云数据和IMU姿态数据，根据激光雷达点云数据和IMU姿态数据基于SLAM算法构建港口三维点云地图和船舶三维模型，并离线保存后存储至船舶进出港三维展示平台；

船舶进出港时位置和姿态的动态更新模块用于获取船舶GPS数据和船舶IMU数据，并通过无线通信方式发送至船舶进出港三维展示平台；

船舶进出港三维展示平台用于加载显示港口三维地图与船舶三维模型构建模块发送的港口三维点云地图和船舶三维模型，接收舶进出港时位置和姿态的动态更新模块发送的船舶GPS数据和船舶IMU数据更新船舶的位置和姿态，以及控制港口和船舶对应信息的显示。

优选地，根据激光雷达点云数据和IMU姿态数据基于SLAM算法构建港口三维地图和船舶三维模型，包括：

使用IMU姿态数据对激光雷达点云数据进行去畸变处理后，对处理后的激光雷达点云数据进行配准，配准后采用迭代最近点方法进行位姿估计、地图和船舶模型构建，最后采用因子图优化，以紧耦合方式融合配准后的激光雷达和IMU测量值，进行三维点云地图和船舶三维模型的生成与保存，依据港口实际经纬度信息对三维点云地图赋予实际地理坐标信息。

优选地，对处理后的激光雷达点云数据进行配准，包括：

设定在t时刻获取的点云Pt，Pi是t时刻的点云Pt中的一个点，将点云Pt投影到预设的距离图像中,确定投影图像的水平方向分辨率和竖直分辨率；

将点云Pt投影到预设的距离图像上后，每个三维空间的点变为二维空间的像素点，获取点Pi对应的像素点到激光雷达传感器的欧氏距离Ri，依据Ri和竖直维度的特性标记出地面点和非地面点；

点云分割之后采用广度优先搜索对点云进行聚类处理，将距离图像分割为多个聚类，同一个聚类的点被标记上唯一的标识，点数少于预设数量点的聚类被作为噪声去除，只保留原始的地面点和相对较大的静态物体；

依据激光雷达每帧点云中每个点的曲率确定角点和平面特征，再依据确定的特征信息进行相邻关键帧之间点云配准。

优选地，使用IMU姿态数据对激光雷达点云数据进行去畸变处理之前，包括：

采用imu_utils开源代码对IMU内参进行标定，标定参数包括陀螺仪和加速度计各自的零偏、标度因数和安装误差；

在船舶上安装激光雷达和9轴IMU传感器，安装时将两个传感器固定在一个基座上，坐标系统一朝向，将基座安装在船头或者船尾；

使用Lidar_IMU_calib方法对激光雷达和IMU之间进行外参标定，将各自传感器的坐标系统一到同一个世界坐标系下；

采集船舶围绕港口航行的激光雷达和IMU数据，采用Ubuntu系统下ROS数据包录制功能使得采集数据过程中激光雷达数据和IMU数据时间同步。

优选地，SLAM算法具体为开源算法LIO-SAM。

优选地，无线通信方式具体为基于UDP通信协议的SOCKET网络通信方式。

优选地，船舶GPS数据具体为船舶经纬度数据，船舶IMU数据具体为船舶与真北方向的夹角信息，获取船舶GPS数据和船舶IMU数据包括：船舶经纬度数据通过船舶上所搭载GPS传感器获取，船舶与真北方向的夹角通过在船舶上装载的9轴IMU的磁力计获取。

优选地，船舶进出港三维展示平台包括ROS节点管理模块、RVIZ显示设置模块以及显示模块，ROS节点管理模块和RVIZ显示设置模块均连接显示模块，显示模块包括三维地图显示窗口、港口和船舶必要信息显示窗口以及人机交互按钮控制窗口，ROS节点管理模块用于处理平台所使用的ROS终端和线程，RVIZ显示设置模块用于对港口三维地图显示参数进行设置。

上述一种基于SLAM技术的船舶进出港三维场景重建与展示系统，包括基于SLAM技术实现港口三维地图和船舶模型构建、船舶GPS和IMU数据通过无线网络通信与船舶进出港三维展示平台进行数据交互以及船舶进出港三维展示平台构建，可移植性好、分辨率高以及可视化性能好，可以简单、真实、形象的展示船舶进出港。

附图说明

图1为本发明一实施例中一种基于SLAM技术的船舶进出港三维场景重建与展示系统的原理框架图；

图2为本发明一实施例中船舶进出港三维实时展示软件主界面图。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

在一个实施例中，如图1所示，一种基于SLAM技术的船舶进出港三维场景重建与展示系统，包括港口三维地图与船舶三维模型构建模块、船舶进出港时位置和姿态的动态更新模块和船舶进出港三维展示平台，船舶进出港时位置和姿态的动态更新模块通过无线通信方式连接船舶进出港三维展示平台；

港口三维地图与船舶三维模型构建模块用于获取多传感器采集到的激光雷达点云数据和IMU姿态数据，根据激光雷达点云数据和IMU姿态数据基于SLAM算法构建港口三维点云地图和船舶三维模型，并离线保存后存储至船舶进出港三维展示平台；

船舶进出港时位置和姿态的动态更新模块用于获取船舶GPS数据和船舶IMU数据，并通过无线通信方式发送至船舶进出港三维展示平台；

船舶进出港三维展示平台用于加载显示港口三维地图与船舶三维模型构建模块发送的港口三维点云地图和船舶三维模型，接收舶进出港时位置和姿态的动态更新模块发送的船舶GPS数据和船舶IMU数据更新船舶的位置和姿态，以及控制港口和船舶对应信息的显示。

具体地，港口三维地图与船舶三维模型的构建，用于在船舶进出港三维展示平台展现港口周围环境信息和进出港的船舶形态。船舶进出港时位置和姿态的动态更新，用于在船舶进出港三维展示平台实时监测船舶进出港的运动状态和方位信息。船舶进出港三维展示平台，用于进行人机交互、港口三维地图和船舶三维模型的加载显示、船舶进出港时与船舶进行数据通信进而实时动态更新船舶的位置和姿态以及港口与船舶相关信息的显示。

进一步地，港口三维地图和船舶模型的构建采用多传感器融合SLAM技术实现。单一传感器SLAM技术在纹理少、四季天气变化、光照剧烈变化、车载条件IMU退化、长走廊、机器人剧烈运动等情况下，很难发挥出应有的效果，从而导致构建地图失败。而船舶在港口航行时场景多为剧烈摇晃、光照剧烈变化等状况，单一传感器SLAM技术无法发挥最大效能，多传感器优势互补可很好弥补缺陷。

在一个实施例中，根据激光雷达点云数据和IMU姿态数据基于SLAM算法构建港口三维地图和船舶三维模型，包括：

使用IMU姿态数据对激光雷达点云数据进行去畸变处理后，对处理后的激光雷达点云数据进行配准，配准后采用迭代最近点方法进行位姿估计、地图和船舶模型构建，最后采用因子图优化，以紧耦合方式融合配准后的激光雷达和IMU测量值，进行三维点云地图和船舶三维模型的生成与保存，依据港口实际经纬度信息对三维点云地图赋予实际地理坐标信息。

在一个实施例中，使用IMU姿态数据对激光雷达点云数据进行去畸变处理之前，包括：

采用imu_utils开源代码对IMU内参进行标定，标定参数包括陀螺仪和加速度计各自的零偏、标度因数和安装误差；

在船舶上安装激光雷达和9轴IMU传感器，安装时将两个传感器固定在一个基座上，坐标系统一朝向，将基座安装在船头或者船尾；

使用Lidar_IMU_calib方法对激光雷达和IMU之间进行外参标定，将各自传感器的坐标系统一到同一个世界坐标系下；

采集船舶围绕港口航行的激光雷达和IMU数据，采用Ubuntu系统下ROS数据包录制功能使得采集数据过程中激光雷达数据和IMU数据时间同步。

在一个实施例中，对处理后的激光雷达点云数据进行配准，包括：

设定在t时刻获取的点云Pt，Pi是t时刻的点云Pt中的一个点，将点云Pt投影到预设的距离图像中,确定投影图像的水平方向分辨率和竖直分辨率；

将点云Pt投影到预设的距离图像上后，每个三维空间的点变为二维空间的像素点，获取点Pi对应的像素点到激光雷达传感器的欧氏距离Ri，依据Ri和竖直维度的特性标记出地面点和非地面点；

点云分割之后采用广度优先搜索对点云进行聚类处理，将距离图像分割为多个聚类，同一个聚类的点被标记上唯一的标识，点数少于预设数量点的聚类被作为噪声去除，只保留原始的地面点和相对较大的静态物体；

依据激光雷达每帧点云中每个点的曲率确定角点和平面特征，再依据确定的特征信息进行相邻关键帧之间点云配准。

在一个实施例中，SLAM算法具体为开源算法LIO-SAM。

在一个实施例中，无线通信方式具体为基于UDP通信协议的SOCKET网络通信方式。

具体地，本发明采用激光雷达和IMU融合SLAM技术实现港口三维地图和船舶模型的构建，其具体步骤如下：

(1)首先对IMU内参进行标定，标定参数主要包括陀螺仪和加速度计各自的零偏、标度因数和安装误差。主流标定方法主要有基于转台的标定、和不依赖转台的标定，其中根据数据求解方式又可分为解析法和最小二乘法。目前开源的标定方法代码主要有imu_utils，本发明采用imu_utils开源代码进行IMU内参标定。

(2)在船舶上安装激光雷达和9轴IMU传感器，安装时将两个传感器固定在一个基座上，坐标系统一朝向，方便后面进行外参标定，最后将其安装在船头或者船尾。

(3)激光雷达和IMU之间进行外参标定，将各自传感器的坐标系统一到同一个世界坐标系下。当前激光雷达和IMU外参标定主流方法主要有瑞士苏黎世联邦理工大学自动驾驶实验室的Lidar_align和浙江大学APRIL实验室开发的Lidar_IMU_calib等。Lidar_align该方法其工作原理为将Lidar数据根据当前假设的状态变量(6DoF参数)变换到Odom系下，构成点云PointCloud，之后针对每一次scan时数据，在下一次scan中通过kdtree的方式寻找最近邻的数据点并计算距离，当总距离最小时则可以认为完全匹配，即计算外参的参数正确，由于该方法为纯IMU积分，无法得到准确的Odom数据，因此其标定结果误差相对较大。而Lidar_IMU_calib由于标定方法精度较高，误差较小，因此在本发明中选择使用该方法进行激光雷达和IMU之间的外参标定。

(4)采集船舶围绕港口航行的激光雷达和IMU数据，在采集数据的过程中需靠近港口岸边特征物体，为激光雷达提供更多有效、丰富的特征信息，以便准确构建高精度港口三维点云地图。此外在采集数据过程中需保证激光雷达数据和IMU数据时间同步，针对该问题本发明采用Ubuntu系统下ROS数据包录制功能，即使用ROS中rosbag record命令同时记录激光雷达点云和IMU姿态数据。

(5)根据采集的港口数据集，离线构建港口高精度三维点云地图和船舶三维模型。三维点云地图构建采用多传感器融合SLAM技术，当前开源成熟的多传感器融合SLAM算法主要有LeGO-LOAM、LIO-SAM、LVI-SAM、ORB-SLAM3等，依据传感器类型、经济成本、港口环境特征等因素本发明选用开源算法LIO-SAM进行港口高精度三维点云地图和船舶三维模型的构建。LIO-SAM算法可融合配准后的激光雷达、IMU和GPS等传感器数据。在进行实际三维点云地图构建的过程中，由于激光雷达在采集数据时，随着船舶运动会导致每一帧点云中的点不是在同一时刻采集的，即不同激光点的坐标系不一致。因此，在进行点云配准之前，首先根据IMU姿态数据对激光雷达点云数据进行去畸变处理。之后将每一帧的点云重投影到图像中，进行地面与非地面点分割，具体分割方法为：首先设定在t时刻获取的点云Pt，Pi是t时刻的点云Pt中的一个点，将该点云投影到一个距离图像中,确定投影图像的水平方向分辨率和竖直分辨率，其中，水平方向分辨率和竖直方向分辨率用来表达投影图像，在将点云投影到图像上后，每个三维空间的点就变为二维空间的像素点，此时获取点Pi对应的像素点到激光雷达传感器的欧氏距离Ri，最后依据Ri和竖直维度的特性标记出地面点和非地面点，其中，竖直维度指激光雷达可以在垂直方向上发射若干条激光，依据激光所打在三维空间中的位置，来确定地面点以下的激光为地面层，地面点以上的激光为非地面层。点云分割之后采用广度优先搜索(BFS)对点云进行聚类处理，将距离图像分割为多个聚类，同一个聚类的点被标记上唯一的标识，点数较少的聚类(少于30点)被作为噪声去除，只保留原始的地面点和相对较大的静态物体。之后依据激光雷达每帧点云中每个点的曲率确定角点和平面特征，再依据确定的特征信息进行相邻关键帧之间点云配准，配准之后采用ICP(Iterative Closest Point)迭代最近点方法估计运动位姿和地图构建。接着采用因子图优化，以紧耦合方式融合配准后的激光雷达和IMU测量值，提升运动位姿估计的准确性。最后进行点云高精度地图的生成与保存。

(6)对港口三维点云地图进行滤波，旋转、平移和赋予点云绝对地理坐标信息等处理。通过SLAM技术构建出的港口三维点云地图和船舶三维模型会存有一定杂点和畸变，本发明通过Cloud Compare开源软件实现对港口三维点云地图和船舶三维模型的滤波、旋转和平移等处理，最后依据港口实际经纬度信息对SLAM技术所构建出的港口三维点云地图赋予实际地理坐标信息。

(7)将处理之后的港口三维点云地图和船舶三维模型离线保存，并提供给船舶进出港三维展示平台。

在一个实施例中，船舶GPS数据具体为船舶经纬度数据，船舶IMU数据具体为船舶与真北方向的夹角信息，获取船舶GPS数据和船舶IMU数据包括：船舶经纬度数据通过船舶上所搭载GPS传感器获取，船舶与真北方向的夹角通过在船舶上装载的9轴IMU的磁力计获取。

具体地，由于船舶航行在港口内，船舶进出港三维展示软件平台在岸基上，因此船舶和软件展示平台之间的数据交互采用无线通信的方式，目前船舶与岸基进行通信已有成熟可靠的产品和方案，在已实现船舶和岸基无线通信的基础上，本发明采用基于UDP通信协议的SOCKET网络通信方式实现船舶和三维展示软件平台之间的数据交互。为实时了解船舶的绝对地理位置和姿态，需获取船舶经纬度数据和船舶与真北方向的夹角信息，船舶经纬度数据通过船舶上所搭载高精度GPS传感器获取，船舶与真北方向的夹角通过在船舶上装载的9轴IMU的磁力计获取，将船舶GPS和IMU数据发送至船舶进出港三维展示软件平台，便能实时获取船舶进出港的位置和信息。

在一个实施例中，船舶进出港三维展示平台包括ROS节点管理模块、RVIZ显示设置模块以及显示模块，ROS节点管理模块和RVIZ显示设置模块均连接显示模块，显示模块包括三维地图显示窗口、港口和船舶必要信息显示窗口以及人机交互按钮控制窗口，ROS节点管理模块用于处理平台所使用的ROS终端和线程，RVIZ显示设置模块用于对港口三维地图显示参数进行设置。

具体地，关于船舶进出港三维展示软件平台构建，硬件要求：一台装有显卡的电脑，运行内存不低于2G、存储内存不低于32G；软件要求：装有ROS平台的Ubuntu操作系统。

船舶进出港三维展示软件平台采用C++、QT和ROS编程实现，该平台主要由SOCKET网络通信、主界面、ROS节点管理和RVIZ显示设置组成。SOCKET网络通信部分采用UDP通信协议，通过C++编程实现，可与外进进行数据交互；主界面采用C++和QT编程并调用RVIZ插件实现，包含三维地图显示窗口、港口和船舶必要信息显示窗口以及人机交互按钮控制窗口；ROS节点管理用于处理平台所使用的ROS终端和线程；RVIZ显示设置用来控制港口三维地图和船舶三维模型的加载，以及地图缩放、平移、旋转和分辨率设置等操作。具体实现步骤如下：

(1)首先通过Qt Designer设计插件设计如图2所示的软件主界面，其包含有显示窗口、按钮、标签、文本框等控件，并根据不同控件所要实现的功能进行排版布局。界面设计搭建完成以后将其转换为C++可调用编辑的程序。

(2)采用C++编程将软件平台主界面中按钮、文本框，标签等控件赋予相应功能。

(3)主界面中船舶进出港动态显示窗口需要加载港口三维点云地图和船舶三维模型，因此需借助ROS中RVIZ插件，RVIZ是一款三维可视化工具，很好的兼容了各种基于ROS框架的软件平台，可用于可视化多种传感器的数据和状态信息。本发明所用激光雷达传感器和IMU传感器，其点云和姿态信息均可以在RVIZ中显示。因此本发明采用C++编程，将RVIZ插件融入到QT主界面显示窗口中，通过RVIZ插件来实现港口三维点云地图的加载显示、移动、缩放、旋转等操作，并实时动态更新显示船舶三维模型的运动姿态。

(4)由于用到RVIZ插件，因此需对ROS节点进行管理，确保RVIZ正常流畅运行。

(5)船舶进出港三维展示软件平台需通过SOCKET网络通信与船舶GPS和IMU数据进行数据交互。SOCKET网络通信的实现，采用C++编写基于UDP通信协议的通信代码，之后将获取的船舶实时数据发送给RVIZ和主界面。

(6)软件平台各部分通过数值传递、通信等方式实现互通互联，最终实现整个船舶进出港三维展示软件平台的构建。

与现有技术相比，本发明的优点在于：

(1)可移植性好

港口三维地图与船舶三维模型的构建，均采用SLAM技术实现，通过在船舶上安装激光雷达和惯性测量单元(IMU)，使船舶围绕港口游走一圈，便可通过多传感器融合SLAM算法构建出港口周围环境的地图，而船舶三维模型采用同样的原理进行构建。不同港口地理环境存在较大不同，但只需提前让船舶围绕港口游走一圈，便可实现不同港口地图的构建，地图构建之后进行离线保存，存储至船舶进出港三维展示软件平台，便可进行港口三维模型的加载显示。该方法操作简单、通用性强、可移植性好。

(2)分辨率高

本发明所提出的基于SLAM技术构建出港口三维地图的方式，由于采用激光雷达传感器采集点云数据，并融合IMU数据实时进行位姿估计，能够构建出高精度、误差相对较小的三维点云地图，因此其分辨率非常高,可达0.1m，且含有真实距离信息。

(3)可视化性能好

本发明所设计的船舶进出港三维展示平台，主要应用在岸基，便于岸基人员实时直观掌握船舶进出港状态。该平台采用C++和QT编程，界面易于设计且响应速度快，为更好的展示三维点云地图和船舶三维模型，在QT里嵌入RVIZ插件，高效实现了三维点云地图的加载、缩放、平移、旋转、拖动等操作，为整个三维展示软件平台提供了一个友善、可视化效果好的界面。

以上对本发明所提供的一种基于SLAM技术的船舶进出港三维场景重建与展示系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。