一种基于三维激光点云的近浅海监测无人艇航行决策方法及装置

本发明属于一种无人艇航行决策方法领域，涉及一种基于三维激光点云的近浅海监测无人艇航行决策方法及装置，具体涉及到近浅海环境下无人艇高精度障碍物感知方法及无人艇全局、局部路径规划方法。

背景技术：

1、在内河、近浅海区域，无人艇的身影出现的越来越频繁。无人艇不仅可搭载水质在线检测仪、测深仪、温度传感器等设备完成水文测量、地质勘察等任务，在治理水面污染、清理水面垃圾方面也有着出色表现。在智慧港口，无人艇可以执行辅助靠泊、系泊入港和自主拖航等业务工作，实现无人货运需求。相应地，在水面工作环境复杂的内河及近浅海区域，无人艇在行驶的过程中，会遇到各种各样的障碍物。如果不能够及时地避开水中的障碍物会对无人艇造成不可逆的危险，导致任务失败。

2、环境信息感知是实现无人艇自主避障的重要基础。无人艇通过环境感知部件感知环境信息，结合上层任务和环境信息来进行决策，从而实现无人艇自主避障的目的。一般情况下，无人艇在开阔水域工作，可以通过gps传感器获取自身当前的速度、位置、航向等信息。但是单独的gps信息不足以保证无人艇安全行驶，水面经常会出现各种障碍物，例如其它艇只、礁石、航标等，如果不能及时发现并避让，非常容易发生危险事故。同时，在近海内河环境中存在着一些gps拒止区域，如桥梁遮挡区域。来自其他艇只或岸边设备的电磁干扰也会导致gps信息暂时失灵或传输不及时，这些问题对于工作在障碍物众多的复杂环境中、需要实时避障的无人艇无疑是致命的。

3、无人艇通过搭载的imu采集到的原始数据，可以解算得出载体的精准位姿变化信息。通过多传感器融合技术融合激光雷达、imu、gps的信息，可以弥补单一传感器的不足，合理应用各个传感器的优点，实现精准的环境信息感知。

4、常用的路径规划方法以电子海图为先验信息，通过提前标记障碍物的位置规划出一条路径来实现避障。但在静态障碍物密集数量众多、动态障碍物出现时间及位置未知的近浅海区域，电子海图能获得的实时环境信息极为有限。因此，如何提升环境感知的精度来实时获取环境中的障碍物信息并实现局部避障和全局路径规划，是保证无人艇在近浅海环境安全作业的重要前提。

技术实现思路

1、为了解决上述问题，本发明采用的技术方案是：一种基于三维激光点云的近浅海监测无人艇航行决策方法，包括以下步骤：

2、初始路径规划：以电子海图为先验信息获取工作区域内静态障碍物的位置信息，通过全局路径规划算法规划出无人艇从起点到终点的最短安全路径；

3、环境感知：通过多传感器融合slam方法获取局部地图及无人艇的位姿信息；

4、障碍物处理：从激光点云地图中识别障碍物信息，将场景中的障碍物与水面分离得到去除水面的障碍物点云，采用预定的聚类算法将与水面点云分离的障碍物点云聚类为一个个的簇；

5、调整初始路径规划：通过局部避障算法使无人艇避开电子海图无法提供的动态障碍物；

6、按全局路径规划算法，规划出来无人艇在工作区域作业时的路径运动，当检测到动态障碍物时，采用动态窗口法进行局部路径规划进行动态避障，以实现无人艇在近浅海区域的自主避障，得到期望航线；

7、期望航线处理：采用预设轨迹优化算法将期望航线平滑处理，得到浅海监测无人艇的运行航线。

8、进一步地：所述通过多传感器融合slam方法获取局部地图及无人艇的位姿信息的过程如下：

9、通过多传感器信息融合框架获取水面信息；

10、所述无人艇搭载的多传感器包括激光雷达里程计、imu、rtk，imu通过积分的方式获取短期内载体的相对位置变化，以得到无人艇的位姿信息，

11、通过多传感器融合算法解算gps提供的绝对位置信息调整imu的状态估计，消除imu的累计误差，得到无人艇精准位姿信息；

12、以imu和gps信息为约束优化激光雷达的点云信息，得到局部地图，通过后端里程计优化算法进行前端里程计的点云精度配准；

13、采用紧耦合方式融合点云数据与imu姿态信息，弥补激光雷达剧烈运动导致位姿估计不准确；

14、采用预设的点云处理算法进行环境中的无关信息数据处理，将干扰因素滤除；

15、同时利用栅格地图算法将点云地图转换为栅格地图；

16、基于图优化理论构建多因子图后端优化模型，将imu因子、gps因子、回环检测因子加入到激光雷达因子中进行优化，实现更高精度的无人艇位姿估计和局部地图。

17、进一步地：所述从激光点云地图中识别障碍物信息的过程如下：

18、采用体素网格对点云数据降采样，仅保留每个体素网格的中心点，减小数据量，同时删除不属于特定区域的点云数据，滤除点云数据包含的噪声点；

19、采用预设的拟合方法拟合点云中的水面，将场景中的障碍物与水面分离；确定聚类点云的边界框，确定点云地图中障碍物的范围；

20、设置预定的栅格边长，将工作区域划分为一个个的栅格，将三维点云地图投影到xy栅格平面。

21、进一步地：所述采用预设的拟合方法拟合环境点云中的水面点云，将场景中的障碍物与水面分离得到去除水面的障碍物点云具体步骤如下：

22、(1)随机选择预设数量的点确定一个平面，构建一个平面模型；

23、(2)计算其他所有点到该平面模型的距离，将距离记录下来与预设距离阈值对比；

24、(3)将在阈值之内的点标记为内点，在阈值之外的点标记为外点；

25、(4)统计内点的数量，若内点的数量超出了预设内点数量阈值，则采用所有内点更新平面模型；

26、(5)重复过程(1)-(4)，直到点云数据利用率达到预设利用率阈值，得到拟合的水面，将除水面外的点云标记为障碍物点云，即实现障碍物点云与水面环境分离。

27、进一步地：所述采用预定的聚类算法将与水面点云分离的障碍物点云聚类为一个个的簇的具体步骤如下：

28、(1)选择某障碍物点为目标点，随机选取一个其他点计算两点之间的距离；

29、(2)若随机选取的点到目标点的距离在预设的距离阈值内，则将随机点标记为目标点，即随机点与目标点是同一类障碍物；

30、(3)若随机点与目标点距离超出了预设阈值，则再选取下一个点计算目标点的距离；

31、(4)障碍物点利用率到达预设利用率时，选取其他未标记的障碍物点，重复上述过程(1)—(3)，多次重复之后可将不同的障碍物分类。

32、一种基于三维激光点云的近浅海监测无人艇航行决策装置，包括：

33、初始路径规划模块：用于以电子海图为先验信息获取工作区域内静态障碍物的位置信息，通过全局路径规划算法规划出无人艇从起点到终点的最短安全路径；

34、环境感知模块：用于通过多传感器融合slam方法获取局部地图及无人艇的位姿信息；

35、障碍物处理模块：用于从激光点云地图中识别障碍物信息，将场景中的障碍物与水面分离得到去除水面的障碍物点云，采用预定的聚类算法将与水面点云分离的障碍物点云聚类为一个个的簇；

36、调整初始路径规划模块：用于通过局部避障算法使无人艇避开电子海图无法提供的动态障碍物；

37、按全局路径规划算法，规划出来无人艇在工作区域作业时的路径运动，当检测到动态障碍物时，采用动态窗口法进行局部路径规划进行动态避障，以实现无人艇在近浅海区域的自主避障，得到期望航线；

38、期望航线处理模块：用于采用预设轨迹优化算法将期望航线平滑处理，得到浅海监测无人艇的运行航线。

39、本发明提供的基于三维激光点云的近浅海监测无人艇航行决策方法及装置，通过无人艇以电子海图为先验信息获取工作区域静态障碍物的位置信息，通过全局路径规划算法规划出一条从起点到终点的最短安全路径；通过多传感器融合slam技术获取高精度的局部地图及无人艇的位姿信息，通过局部避障算法使无人艇避开电子海图无法提供的动态障碍物。无人艇在工作区域作业时按全局路径规划算法规划出来的路径运动，当检测到动态障碍物时进行动态避障，以实现无人艇在近浅海区域的自主避障；

40、包括：通过多传感器信息融合框架获取丰富的水面信息；利用imu、rtk的信息互补，通过多传感器融合算法获取载体的精确位姿信息；通过imu、rtk的信息来优化激光雷达信息，弥补载体剧烈运动时造成的点云畸变，获取精确局部地图；获取处理到的局部点云地图，去除包含障碍物点云的水面点云；将去除水面点云的地图栅格化，获得二维局部栅格地图；在栅格地图的基础上实现无人艇的局部避障，避开水面上的动态障碍物；在栅格化的电子海图的基础上进行全局路径规划，规划出从起点到终点的期望路径。同时，采用轨迹优化算法来避免轨迹出现尖锐拐点；

41、与现有技术相比，具有以下优点：本发明的有益效果在于：通过多传感器融合slam技术实现了高精度的动态障碍物感知及局部高精地图获取，通过rtk补偿imu以获取无人艇的精准位姿信息以避开近浅海环境下复杂动态障碍物，通过r-s优化方法来避免轨迹出现尖锐拐点，使路径符合无人艇动力学。无人艇以全局路径规划算法规划的期望航线航行，并在航行的过程中时刻保持能避开静态障碍物如礁石和动态障碍物如浮漂的能力。