CN116540727A - 基于多传感器融合定位的欠驱动无人水面艇自主靠泊方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，该方法通过多传感器融合slam系统构建港口环境下的点云地图，为无人船自动停泊过程中定位与自主避障提供信息。靠干扰能力强，在某类传感器信号缺失或信号弱的情况下依然能保证定位精度；不依赖岸基端设备的牵引与信息交互，拓展性、可移植性强，可应用在大部分船舶上而无需对船身进行改造。由于环境点云在无人船的行进过程中是不断获取的，因此通过点云地图定位的方式能够得到一直更新的船体位姿信息。从而提升了低速小位移时的船体位姿的准确性，可搭配特定港口停泊装置或在任意港口水岸环境下停泊。

Description

基于多传感器融合定位的欠驱动无人水面艇自主靠泊方法

技术领域

本发明涉及无人艇航行控制领域，尤其涉及一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法。

背景技术

水面无人船(UnmannedSurfaceVehicle，USV)根据其自主性可分为遥控、半自主和自主三种，按照预设任务规划路径航行，具有机动灵活、隐蔽性好和成本低等特点。因为其在安全、效率、回报率等方面较好的表现，所以在军事方面利用无人艇代替人执行危险的工作，在民用方面利用USV有效减少人员费用的支出和提高船舶航行的安全性。近年来无人船在海事监管、水质检测、巡检巡护、地形勘测、搜寻救助等领域的应用比重逐年增加。无人船欠驱动的特点使其受环境影响巨大，由于水面工作环境复杂多变且往往伴随风浪流的扰动，因此对执行任务过程中的安全性指标要求十分严格。

无人船在开阔水域的应用技术较为成熟，但在离港靠泊的过程中还是需要人工来完成，自主靠泊技术是船舶应用研究技术中最复杂的技术之一，目前发展处于起步阶段。无人艇无论是在进港，还是在靠近海上生产平台时都需要进行靠泊操纵，由于港口水域环境复杂多变船只较多等原因，船舶自主靠离泊技术仍是研究无人船舶技术的难点问题，也已成为无人水面艇全自主作业的瓶颈问题。无人船如果需要实现完全的自主性，不仅要在执行任务时能够自主地依据实际的任务需求来进行任务相关的操作，而在完成任务之后也应能实现自主的、无人工干预的停泊，在拥有了自主停泊的功能之后，才能称得上是真正的无人艇。在一些特殊应用领域还对无人船的出动时间提出了要求，根据《国家水上交通安全和救助系统布局规》制定的监管目标，巡逻船航行前准备时间不得超过五分钟，要能在12h之内到达离岸200nmile内的任何水域，在90min之内到达离岸50nmile内的重点水域。

现有技术中依靠该方案难以确保以较高的靠泊精度完成靠泊任务。依靠人工选择设置目标点的停泊方案，对定位精度的要求很高，并且限制了无人艇自主性能，不能推进无人艇应用进一步发展。因此，如何能在减少对岸基设备以及操作人员依赖的情况下，依靠无人船自身的定位系统确保靠泊精度，并且获得周围环境动态变化的相对信息提供安全稳定可靠的自主停靠泊方案是急需突破的瓶颈。如果无人艇能依据泊位标志物来自主停泊，执行任务时自主离港，将会促进无人艇在巡逻安保，海洋牧场等领域大规模应用和发展。

在通行区域狭窄、水面障碍物动态变换的港口，无人船的航行与停泊需要感知环境变化以及精确定位的能力。港口水域中分布着航道、泊位、航标等通航环境要素，同时也是船舶、货物、船员等海事监管要素的聚集之地，船舶的货物作业、加油作业等操作行为大多发生在港口水域。传统依靠GPS提供定位信息引导的无人船，由于进离港时靠近岸边较近，可能会导致船体的GPS信号受到岸或其他船舶大型电子设备的干扰。恶劣天气状况以及上方有物体或高大建筑物遮挡的水面场景使得卫星定位信号弱，出现跳变现象使得准确度进一步受到影响甚至导致触岸事故的发生。在定位精度受到影响甚至没有卫星信号的情况下如何能够获得精确度较高的定位信息是无人船定位及自动停泊领域的技术难题。

当前无人船的自主停泊存在定位精度差，无法将无人船准确的停靠在指定泊位的问题。大部分无人艇的入港只能在低航速、低海况下人工操作进行，搭载了先进传感器具有自主性的无人船往往也是由操作员通过操作经验对船舶状况、航行状态做出判断，手动遥控操作无人船至指定位置来回收。但是在障碍物较多，通行区域阻塞的港口环境下，人工操作也很难准确及时的执行，对操作人员的技术和素质要求很高。且在回收过程中因为缺少全局规划和控制的能力，出现决策失误意外操作导致船舶碰撞而损毁搭载设备或样品的情况，造成不必要的人力、物力和财力损失。即便有部分无人船能完成自主停泊任务，也必须是在非常理想的环境下进行的，有些是需要岸基端架设相应设备进行配合引导，有些是需要在岸基上安装大量传感器来辅助船只进行定位，还有部分是需要在船舶端额外增设相应设备，对于无人船的负载能力是额外的负担。

发明内容

根据现有技术存在的问题，本发明公开了一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，具体包括如下步骤：

无人艇收到入港指令后，按照预先设定的程序进行泊位感知和定位从而执行入港过程，使用船身搭载的GNSS导航设备实时获取无人船的相对位置和航向信息；

基于无人船位置信息确定无人艇在全局地图中的位置，采用路径规划算法产生离散坐标点，将船舶转弯半径和角加速度固有属性作为约束，对路径拐点处进行平滑处理，通过PID算法控制无人船沿着优化后的路径轨迹行进；

采用激光雷达传感器检测停泊点标志物信息，根据回波强度进行实时判断是否找到停泊位置标识，当无人船搭载激光雷达识别到两标志物后，进行判断泊位是否空闲，如果是则执行靠泊任务，如果否则继续寻找满足条件的泊位、或等待其他指令的传递；

当有保存好的先验点云地图时，首先通过GPS提供的位置信息加载局部点云地图，利用船体激光雷达感知系统获取无人船当前所处位置周围的环境点云原始信息，将提取特征值与之前保存的局部点云地图进行匹配，确定无人船当前处于点云地图中的位姿，从而基于码头标志物在点云地图中的位置确定无人船在进港或离港时的船体位姿信息，最后根据船体位姿信息控制无人船驶进或驶离码头。

进一步的，无人艇进行泊位感知和定位时：利用船载激光雷达通过对比回波，根据反射率的不同检测出待停泊点上方的标志物并将其作为路径规划的终点，通过GPS获取绝对位置信息，通过标志物在雷达点云坐标系下的点云确定与无人船的相对位置关系。

进一步的，无人船根据泊位、自身的相对位姿信息、起点终止点的绝对经纬度信息，自主规划出从当前位置至目标停泊点的无碰触路径；无人船到泊位的自主回航路径规划分为两阶段，第一阶段是在无人艇接收到停泊指令后通过电子海图获取无人船当前所处位置周围的全局地图信息，在港口内按照全局路径规划算法规划路径快速行进，同时通过船体激光雷达识别周围水面障碍物和码头标志物的环境信息；第二阶段是检测到预设的反光标志物后根据激光雷达获取的点云地图解算船舶与标记物的位姿信息，规划出符合船舶运动学特征的轨迹。

进一步的，通过两标志物中点和当前无人船艏向计算出航向偏差角，当前位置与规划路径的偏差计算出偏航距离，将航向偏差角和偏航距离等信息输入路径规划模块中，通过PID算法来实时动态调整无人水面艇航速和航向使其驶向泊位。

进一步的，通过将GPS、IMU、GPS信息融合构建点云地图获取无人船与停泊点上方标志物的相对位置，对船舶进行精准定位从而调整无人船航向实现精准停泊。

由于采用了上述技术方案，本发明提供的一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，该方法通过多传感器融合slam系统构建港口环境下的点云地图，为无人船自动停泊过程中定位与自主避障提供信息。靠干扰能力强，在某类传感器信号缺失或信号弱的情况下依然能保证定位精度；不依赖岸基端设备的牵引与信息交互，拓展性、可移植性强，可应用在大部分船舶上而无需对船身进行改造；不易受复杂环境影响，使用激光雷达作为感知泊位信息的传感器，与摄像头相比，最大程度上的降低了环境中光照、雨雪、海雾等因素的影响，同时3D激光雷达具有360°的全向探测范围，不存在因船舶姿态导致摄像头可视范围内无停泊点需重新规划路径的问题。由于环境点云在无人船的行进过程中是不断获取的，因此通过点云地图定位的方式能够得到一直更新的船体位姿信息，当GPS系统信号丢失或不可靠时也可得到无人船当前的位置信息和姿态信息。从而提升了低速小位移时的船体位姿的准确性，可搭配特定港口停泊装置或在任意港口水岸环境下停泊。

本发明在船舶的前中后以及船底分别安装有3D激光雷达、毫米波雷达与水下超声波测深仪，可以将船体与环境中的障碍物距离信息实时向运算处理单元进行反馈，在可行区域狭窄的港口环境实现了无人船精准姿态靠泊，从而减轻使用者重复操作的劳动强度以及因主观判断偏差造成的意外事故，进一步摆脱对人工操作的依赖，增加无人船的智能性与自主性。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明方法的流程图

图2为本发明方法中基于先验地图靠泊过程示意图

图3为本发明方法中停泊控制流程图

图4为本发明方法中图无人船停泊过程示意图

图5为本发明方法中多传感器融合建图示意图。

图6为本发明方法中图激光雷达探测范围与安全距离示意图。

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：

自主停泊不仅需要在泊位点处的精准位姿靠泊能力，还需要在复杂港口环境下的路径规划与避障能力。在复杂港口环境下的通行首先需要对环境中静态以及动态障碍物信息实时感知获取，接下来在感知的基础上划分出可行区域进行路径规划。本发明在港口环境下进行路径规划时没有将船舶简化做质点处理，而是考虑到无人船欠驱动的特点，船舶大小、转弯半径等固有属性以及最大加速度、角速度、速度等运动学特征，使规划的轨迹能应用在狭窄港口环境下。

鉴于已有技术存在的不足，本发明公开了一种基于IMU、GPS和激光雷达数据融合的多传感器融合slam环境感知、路径规划方法及无人艇传感器搭载方案，是一种可以自动停泊至指定位置无需岸基端设备配合的无人船自动停泊系统及方法，属于无人艇数据融合感知、路径规划、停泊、无人艇应用技术领域。用于解决无人船停泊过程中对环境感知信息缺失、定位精度差需要人工操作干预以及依赖岸基端设备的停泊方案应用受限的问题。

如图1和图2所示的一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，具体包括如下步骤：

S1:操作人员通过岸上终端人工启动或无人艇完成预设任务归港后自主启动回航程序，在无人艇接收到入港指令后，按照预先设定的入港程序执行；

S2:由于在无人船离岸边较远时，控制无人船航行不需要太高的定位精度，并且此时由于特征点稀疏无法构建精细的点云地图，因此这一阶段使用船身搭载的GNSS导航设备实时获取无人船的位置和航向作为引导，其提供的位置定位精度可以满足该阶段的精度需求。如图3至图5所示，首先根据GPS得到的位置信息确定无人艇在全局地图中的位置，利用基于A*算法改进的LOSLAZYA*算法产生离散坐标点，接下来根据无人船通过海上试验获取得到的船舶转弯半径，角加速度等固有属性作为约束，对路径拐点处进行平滑处理，通过PID算法控制无人船沿着优化后的路径轨迹行进。无人艇沿着航道快速前行，行进时使用激光雷达传感器检测停泊点标志物，数据分析根据回波强度进行实时判断是否找到停泊位置标识，由于3D激光雷达具有360°感知范围，所以行进过程中的船体姿态对观测并没有任何影响。当无人船搭载激光雷达识别到两标志物后，进行判断泊位是否空闲，如果满足上述条件进入下一个阶段执行低速回港任务。否则重复此过程。

S3:分为有先验点云地图和对环境完全未知两种情况

当有保存好的先验点云地图时，首先通过GPS提供的位置信息加载局部点云地图，加下来利用船体激光雷达感知系统获取无人船当前所处位置周围的环境点云原始信息，然后提取特征值与之前保存的局部点云地图进行匹配，确定无人船当前处于点云地图中的位姿，进而基于码头标志物在点云地图中的位置确定无人船在进港或离港时的船体位姿信息；最终根据船体位姿信息控制无人船驶进或驶离码头。环境信息包括水面和码头标志物，码头标志物包括诸如停泊位置标志、码头建筑物、标杆、竖旗、吊臂等，水面标志物包括停放的其他船舶、水面浮标、缆绳等。

没有先验点云地图时通过将激光雷达、GPS和IMU三者的数据融合实时创建泊位点周围点云地图，从中提取出泊位标志物并将其作为路径规划的终点，构建更加可靠的环境感知定位系统，精准获取环境信息以及自身位姿信息。选择矩形反光贴为标志物(标志物可以是任意形状)，各标志物等距且共线布置在返航区域对应的岸基上，摆放位置与岸基呈平行关系，两个标志物之间的点为两者的中心点。靠泊点处提前布置好标记物，当无人船执行靠泊程序时，通过识别标志物确定待停泊点的位置信息。选择距离两泊位标志物中点前方1.5米处为目标终点，两标志物之间为靠泊区域，该区域中心为期望位置。在获得目标泊位位置与泊位环境信息之后，根据入泊起点的位置和入泊终点的位置以及当前船舶艏向规划路径。在靠泊过程中要充分考虑在海洋环境扰动下无人艇由于低速行驶产生操纵性能变差问题以及船舶欠驱动的特性，考虑无人艇艏向以及实船试验得到的船舶转弯半径、最大线速度、最大角速度等船舶自身运动学特征作为约束对规划得到的路径进行优化，从而将产生一段光滑、安全且运动方向改变次数少符合无人船运动特性的轨迹，解决了靠泊时容易出现的拐角过大问题。

再具体靠泊点足够近后，通过水下超声波探测仪确定待停泊点水深以及是否存在障碍物，如果满足条件，则进行后续靠泊，如图6所示。.

S4:当到达停泊点附近，判断是否到达停泊位置，到达则停止，判断靠泊成功停止条件为艇身方向与岸线垂直方向夹角小于15°，船体前沿距岸小于30厘米，且速度为0。否则根据船舶的速度和角度控制船载执行机构动作，从而实现船舶的靠泊过程中的动态调节，减少因停靠不到位多次规划引起的碰撞现象。当无人船在停泊区域内时，通过高精度点云地图能很轻易的实现厘米级的定位，根据船舶实时更新的位置信息保证悬停锁定在停泊区域内，等待下一次执行任务或工作人员前来断电回收。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，其特征在于包括：

无人艇收到入港指令后，按照预先设定的程序进行泊位感知和定位从而执行入港过程，使用船身搭载的GNSS导航设备实时获取无人船的相对位置和航向信息；

基于无人船位置信息确定无人艇在全局地图中的位置，采用路径规划算法产生离散坐标点，将船舶转弯半径和角加速度固有属性作为约束，对路径拐点处进行平滑处理，通过PID算法控制无人船沿着优化后的路径轨迹行进；

采用激光雷达传感器检测停泊点标志物信息，根据回波强度进行实时判断是否找到停泊位置标识，当无人船搭载激光雷达识别到两标志物后，进行判断泊位是否空闲，如果是则执行靠泊任务，如果否则继续寻找满足条件的泊位、或等待其他指令的传递；

当有保存好的先验点云地图时，首先通过GPS提供的位置信息加载局部点云地图，利用船体激光雷达感知系统获取无人船当前所处位置周围的环境点云原始信息，将提取特征值与之前保存的局部点云地图进行匹配，确定无人船当前处于点云地图中的位姿，从而基于码头标志物在点云地图中的位置确定无人船在进港或离港时的船体位姿信息，最后根据船体位姿信息控制无人船驶进或驶离码头。

2.根据权利要求1所述的基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，其特征在于：无人艇进行泊位感知和定位时：利用船载激光雷达通过对比回波，根据反射率的不同检测出待停泊点上方的标志物并将其作为路径规划的终点，通过GPS获取绝对位置信息，通过标志物在雷达点云坐标系下的点云确定与无人船的相对位置关系。

3.根据权利要求1所述的基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，其特征在于：无人船根据泊位、自身的相对位姿信息、起点终止点的绝对经纬度信息，自主规划出从当前位置至目标停泊点的无碰触路径；无人船到泊位的自主回航路径规划分为两阶段，第一阶段是在无人艇接收到停泊指令后通过电子海图获取无人船当前所处位置周围的全局地图信息，在港口内按照全局路径规划算法规划路径快速行进，同时通过船体激光雷达识别周围水面障碍物和码头标志物的环境信息；第二阶段是检测到预设的反光标志物后根据激光雷达获取的点云地图解算船舶与标记物的位姿信息，规划出符合船舶运动学特征的轨迹。

4.根据权利要求1所述的基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，其特征在于：通过两标志物中点和当前无人船艏向计算出航向偏差角，当前位置与规划路径的偏差计算出偏航距离，将航向偏差角和偏航距离等信息输入路径规划模块中，通过PID算法来实时动态调整无人水面艇航速和航向使其驶向泊位。

5.根据权利要求1所述的基于多传感器融合SLAM定位的欠驱动无人水面艇自主靠泊方法，其特征在于：通过将GPS、IMU、GPS信息融合构建点云地图获取无人船与停泊点上方标志物的相对位置，对船舶进行精准定位从而调整无人船航向实现精准停泊。