一种基于多雷达感知的无人船避障方法

摘要

本发明公开一种基于多雷达感知的无人船避障方法，包括：多雷达数据融合：控制多雷达异步启动，获取各雷达的点云数据，对点云数据打上时间戳并与传感器数据进行数据融合，获取各雷达的融合数据包；雷达点云滤波：根据融合数据包对各雷达时间同步后进行点云融合并滤波，获取滤波后的雷达融合点云；雷达栅格图建立：建立栅格图坐标系，并将雷达融合点云投影至其上，获得雷达栅格图；雷达栅格图滤波处理：于雷达栅格图中提取局部栅格图并滤除离散杂点；避障启动判断：当检测路线存在危险点，启动避障；避障目标点选取：获取路线的危险点，选取避障目标点；避障路线规划：于局部栅格图中根据船当前位置及避障目标点，利用启发式搜索获取避障路线。

权利要求

1.一种基于多雷达感知的无人船避障方法，其特征在于，包括以下步骤：多雷达数据融合：控制多雷达异步启动，获取各雷达的点云数据，对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包；

雷达点云滤波：根据各雷达的融合数据包对各雷达进行时间同步，对时间同步后的各雷达的点云数据进行融合，获取融合点云，对融合点云进行滤波处理，获取滤波后的雷达融合点云；

雷达栅格图建立：建立栅格图坐标系，将雷达融合点云投影至栅格图坐标系，获得雷达栅格图；其中，栅格图坐标系中被雷达融合点云投影覆盖的栅格位置标记为障碍点；

雷达栅格图滤波处理：根据船当前位置于雷达栅格图中提取局部栅格图，对提取获得的局部栅格图预处理以滤除离散杂点；

避障启动判断：检测路线是否存在危险点，若是，启动避障，选取避障目标点，进行避障规划；

避障目标点选取：获取路线的危险点，根据获得的危险点选取避障目标点；

避障路线规划：于局部栅格图中根据船当前位置及避障目标点，利用启发式搜索获取避障路线。

2.根据权利要求1所述的基于多雷达感知的无人船避障方法，其特征在于，所述多雷达数据融合的步骤中的控制多雷达异步启动的步骤具体为：控制多个雷达依序间隔预设时间启动，所述预设时间小于雷达的工作周期。

3.根据权利要求1所述的基于多雷达感知的无人船避障方法，其特征在于，所述多雷达数据融合的步骤中的对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包的步骤具体包括：获取各雷达的点云数据，对各雷达的点云数据打上对应的时间戳，根据IMU传感器及GPS传感器的数据获取时间戳对应的时刻船的位姿信息并关联融合至相应的点云数据，获得各雷达的融合数据包；

对各雷达的融合数据包根据对应的时间戳按照时间顺序进行排序，获取各雷达的数据队列。

4.根据权利要求3所述的基于多雷达感知的无人船避障方法，其特征在于，所述雷达点云滤波的步骤具体包括：对各雷达的融合数据包进行滤波；

选取其中一个雷达作为主雷达，剩余的雷达设为子雷达，主雷达的点云数据所在的坐标系作为主坐标系，根据主雷达的融合数据包的时间戳对子雷达的融合数据包进行时间同步；

将时间同步的所有子雷达的点云数据及主雷达的点云数据合并至主坐标系中，获得融合点云；

使用聚类的方法对融合点云进行滤波处理，滤除融合点云中的离群点，获得点云簇，作为滤波后的雷达融合点云。

5.根据权利要求4所述的基于多雷达感知的无人船避障方法，其特征在于，所述雷达栅格图建立的步骤具体为：栅格图坐标系建立：以正北为y轴正方向，正东为x轴正方向，并以船自身的初始的GPS坐标作为原点，建立栅格图坐标系；

雷达栅格图获取：根据融合点云的朝向、船自身的GPS坐标及栅格图坐标系的原点，计算出主坐标系相对于栅格图坐标系的旋转矩阵及平移向量，将融合点云投影至栅格图坐标系，获得雷达栅格图。

6.根据权利要求5所述的基于多雷达感知的无人船避障方法，其特征在于，所述雷达栅格图建立的步骤前还包括：判断是否已存在雷达栅格图；

若是，进行雷达栅格图维护。

7.根据权利要求6所述的基于多雷达感知的无人船避障方法，其特征在于，所述进行雷达栅格图维护的步骤具体为：根据当前时刻的船头朝向、船自身的GPS坐标及雷达感知范围计算出栅格图维护区域；

若栅格图维护区域内的已存在的障碍点在雷达的连续多个工作周期内未被融合点云投影覆盖，取消该栅格为障碍点的标记；若栅格图维护区域内的非障碍点被融合点云投影覆盖，则将该非障碍点更新标记为障碍点；

获得维护更新后的雷达栅格图。

8.根据权利要求1所述的基于多雷达感知的无人船避障方法，其特征在于，所述雷达栅格图滤波处理的步骤具体包括：根据当前时刻的船自身的GPS坐标，于栅格图中提取以该GPS坐标为中心的径向尺寸大于雷达探测距离的局部栅格图；

对提取获得的局部栅格图进行腐蚀操作，滤除离散杂点；

对滤除离散杂点后的局部栅格图进行卷积膨胀。

9.根据权利要求1所述的基于多雷达感知的无人船避障方法，其特征在于，所述避障启动判断的步骤具体包括：于预处理后的局部栅格图上利用路径索引查找船的航行路径的所有路径点；

获取与各路径点的沿路径方向的距离大于雷达探测距离的点作为对应的检测点，所有检测点形成局部路线；

判断检测点是否为危险点；

若是，启动避障，选取避障目标点，进行避障规划。

10.根据权利要求9所述的基于多雷达感知的无人船避障方法，其特征在于，所述避障目标点选取的步骤具体包括:获取局部路线的最后一个危险点；

判断该局部路线的最后一个危险点是否为全局路线的尾点；

若是，选取局部路线的最后一个非危险点的检测点作为避障目标点；

若否，于全局路线中沿路径方向在该危险点后的所有路径点中选取距离该危险点预设长度后的第一个非危险点的检测点为避障目标点。

说明书

一种基于多雷达感知的无人船避障方法

技术领域

[0001] 本发明涉及无人船避障技术领域，尤其涉及一种基于多雷达感知的无人船避障方法。

背景技术

[0002] 近年来，随着无人驾驶技术的迅猛发展，无人驾驶市场不断增长，以无人船为例，各类无人船逐渐被推广至水质检测、水面维护、桥梁检测、水体测绘、海事搜救等多种作业场景下，辅助人们工作。感知是无人驾驶技术的基础，在复杂的水面环境下,无人船需要具备灵敏的环境感知能力。由于现有的无人船环境感知能力有限，通常需要人工操作辅助其作业任务，无法实现全天候完全无人自主作业，难以大范围推广与应用。作为自主执行任务中必不可少的一部分，良好的自主避障能力有助于提升无人船的自主性，更好地执行任务。

[0003] 现有无人船大部分使用视觉进行环境感知，但对于水面环境，水面强光反射容易对视觉传感器造成干扰。由于画面中大部分被天空和天空的倒影占据，水面场景容易面临更为严重的过度曝光问题，对于视觉传感器的选型和算法的鲁棒性提出了更高的要求。而且，容易受雨雾天气干扰。水面环境下，船只在雨中应仍能进行自主化作业，而视觉传感器对在雨天以及水面有雾天气下往往表现不佳，图像模糊或者被水珠遮挡。

发明内容

[0004] 本发明所要解决的技术问题是提供一种基于多雷达感知的无人船避障方法，以利用多雷达的数据融合技术，扩大感知范围，提高抗干扰能力，提高避障精度。

[0005] 为解决上述技术问题，本发明的目的是通过以下技术方案实现的：提供一种基于多雷达感知的无人船避障方法，包括以下步骤：

[0006] 多雷达数据融合：控制多雷达异步启动，获取各雷达的点云数据，对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包；

[0007] 雷达点云滤波：根据各雷达的融合数据包对各雷达进行时间同步，对时间同步后的各雷达的点云数据进行融合，获取融合点云，对融合点云进行滤波处理，获取滤波后的雷达融合点云；

[0008] 雷达栅格图建立：建立栅格图坐标系，将雷达融合点云投影至栅格图坐标系，获得雷达栅格图；其中，栅格图坐标系中被雷达融合点云投影覆盖的栅格位置标记为障碍点；

[0009] 雷达栅格图滤波处理：根据船当前位置坐标于雷达栅格图中提取局部栅格图，对提取获得的局部栅格图预处理以滤除离散杂点；

[0010] 避障启动判断：检测路线是否存在危险点，若是，启动避障，选取避障目标点，进行避障规划；

[0011] 避障目标点选取：获取路线的危险点，根据获得的危险点选取避障目标点；

[0012] 避障路线规划：于局部栅格图中根据船当前位置及避障目标点，利用启发式搜索获取避障路线。

[0013] 其进一步技术方案为：所述多雷达数据融合的步骤中的控制多雷达异步启动的步骤具体为：控制多个雷达依序间隔预设时间启动，所述预设时间小于雷达的工作周期。

[0014] 其进一步技术方案为：所述多雷达数据融合的步骤中的对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包的步骤具体包括：

[0015] 获取各雷达的点云数据，对各雷达的点云数据打上对应的时间戳，根据IMU传感器及GPS传感器的数据获取时间戳对应的时刻船的位姿信息并关联融合至相应的点云数据，获得各雷达的融合数据包；

[0016] 对各雷达的融合数据包根据对应的时间戳按照时间顺序进行排序，获取各雷达的数据队列。

[0017] 其进一步技术方案为：所述雷达点云滤波的步骤具体包括：

[0018] 对各雷达的融合数据包进行滤波；

[0019] 选取其中一个雷达作为主雷达，剩余的雷达设为子雷达，主雷达的点云数据所在的坐标系作为主坐标系，根据主雷达的融合数据包的时间戳对子雷达的融合数据包进行时间同步；

[0020] 将时间同步的所有子雷达的点云数据及主雷达的点云数据合并至主坐标系中，获得融合点云；

[0021] 使用聚类的方法对融合点云进行滤波处理，滤除融合点云中的离群点，获得点云簇，作为滤波后的雷达融合点云。

[0022] 其进一步技术方案为：所述雷达栅格图建立的步骤具体为：

[0023] 栅格图坐标系建立：以正北为y轴正方向，正东为x轴正方向，并以船自身的初始的GPS坐标作为原点，建立栅格图坐标系；

[0024] 雷达栅格图获取：根据融合点云的朝向、船自身的GPS坐标及栅格图坐标系的原点，计算出主坐标系相对于栅格图坐标系的旋转矩阵及平移向量，将融合点云投影至栅格图坐标系，获得雷达栅格图。

[0025] 其进一步技术方案为：所述雷达栅格图建立的步骤前还包括：

[0026] 判断是否已存在雷达栅格图；

[0027] 若是，进行雷达栅格图维护。

[0028] 其进一步技术方案为：所述进行雷达栅格图维护的步骤具体为：

[0029] 根据当前时刻的船头朝向、船自身的GPS坐标及雷达感知范围计算出栅格图维护区域；

[0030] 若栅格图维护区域内的已存在的障碍点在雷达的连续多个工作周期内未被融合点云投影覆盖，取消该栅格为障碍点的标记；若栅格图维护区域内的非障碍点被融合点云投影覆盖，则将该非障碍点更新标记为障碍点；

[0031] 获得维护更新后的雷达栅格图。

[0032] 其进一步技术方案为：所述雷达栅格图滤波处理的步骤具体包括：

[0033] 根据当前时刻的船自身的GPS坐标，于栅格图中提取以该GPS坐标为中心的径向尺寸大于雷达探测距离的局部栅格图；

[0034] 对提取获得的局部栅格图进行腐蚀操作，滤除离散杂点；

[0035] 对滤除离散杂点后的局部栅格图进行卷积膨胀。

[0036] 其进一步技术方案为：所述避障启动判断的步骤具体包括：

[0037] 于预处理后的局部栅格图上利用路径索引查找船的航行路径的所有路径点；

[0038] 获取与各路径点的沿路径方向的距离大于雷达探测距离的点作为对应的检测点，所有检测点形成局部路线；

[0039] 判断检测点是否为危险点；

[0040] 若是，启动避障，选取避障目标点，进行避障规划。

[0041] 其进一步技术方案为：所述避障目标点选取的步骤具体包括：

[0042] 获取局部路线的最后一个危险点；

[0043] 判断该局部路线的最后一个危险点是否为全局路线的尾点；

[0044] 若是，选取局部路线的最后一个非危险点的检测点作为避障目标点；

[0045] 若否，于全局路线中沿路径方向在该危险点后的所有路径点中选取距离该危险点预设长度后的第一个非危险点的检测点为避障目标点。

[0046] 本发明的有益技术效果在于：本发明一种基于多雷达感知的无人船避障方法通过控制多雷达异步启动，使得多个雷达的收发时间错开，可有效消除多个同频段雷达相互干扰的问题，通过多雷达数据融合扩大感知范围，增强抗干扰能力，并通过获取栅格图的形式规划避障路线，提高避障精度，而且，提取局部栅格图以进行避障规划，可减少计算复杂度，提升避障规划的效率。

附图说明

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

[0048] 图1为本发明实施例提供的一种基于多雷达感知的无人船避障方法的流程示意图；

[0049] 图2为本发明实施例提供的一种基于多雷达感知的无人船避障方法的多雷达数据融合步骤的子流程示意图；

[0050] 图3为本发明实施例提供的一种基于多雷达感知的无人船避障方法的雷达点云滤波步骤的具体流程示意图；

[0051] 图4为本发明实施例提供的一种基于多雷达感知的无人船避障方法的雷达栅格图建立步骤的具体流程示意图；

[0052] 图5为本发明实施例提供的一种基于多雷达感知的无人船避障方法的雷达栅格图滤波处理步骤的具体流程示意图；

[0053] 图6为本发明实施例提供的一种基于多雷达感知的无人船避障方法的避障启动判断步骤的具体流程示意图；

[0054] 图7为本发明实施例提供的一种基于多雷达感知的无人船避障方法的避障目标点选取步骤的具体流程示意图；

[0055] 图8为本发明另一实施例提供的一种基于多雷达感知的无人船避障方法的流程示意图。

具体实施方式

[0056] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0057] 应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和 “包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

[0058] 还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/ 或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

[0059] 请参阅图1所示，图1为本发明实施例提供的一种基于多雷达感知的无人船避障方法的流程图。所述基于多雷达感知的无人船避障方法包括以下步骤：

[0060] 步骤S11、多雷达数据融合：控制多雷达异步启动，获取各雷达的点云数据，对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包。其中，通过控制多雷达异步启动，可使得多个雷达的收发时间错开，有效消除多个同频段雷达相互干扰的问题，通过多雷达数据融合扩大感知范围，增强抗干扰能力。所述IMU传感器及GPS传感器的数据为IMU传感器及GPS传感器检测获得的数据信息，以可获取船的位姿信息。所述融合数据包包括点云数据、时间戳及位姿信息。

[0061] 具体地，所述步骤S11中的雷达可采用毫米波雷达，利用毫米波雷达的波长短、频带宽及穿透能力强的特点，使得雷达在雨雾等恶劣天气条件下的抗干扰能力强，从而雷达信号衰减相对较弱，对于光照和天气等外界环境的鲁棒性较好，能够有效保障无人船在各种环境下的稳定运行。

[0062] 具体地，所述步骤S11中的控制多雷达异步启动的具体步骤为：控制多个雷达依序间隔预设时间启动，所述预设时间小于雷达的工作周期。通过控制多个雷达依序间隔预设时间启动可使得多个雷达的发射时间及接收时间错开，从而消除多个同频段的雷达相互干扰的问题，可有效防止信息丢失或者出现虚假目标点的问题。

[0063] 步骤S12、雷达点云滤波：根据各雷达的融合数据包对各雷达进行时间同步，对时间同步后的各雷达的点云数据进行融合，获取融合点云，对融合点云进行滤波处理，获取滤波后的雷达融合点云。

[0064] 步骤S13、雷达栅格图建立：建立栅格图坐标系，将雷达融合点云投影至栅格图坐标系，获得雷达栅格图；其中，栅格图坐标系中被雷达融合点云投影覆盖的栅格位置标记为障碍点。障碍点是指栅格图中被雷达融合点云投影覆盖的栅格，则非障碍点是指栅格图中未被融合点云投影覆盖的栅格。

[0065] 步骤S14、雷达栅格图滤波处理：根据船当前位置于雷达栅格图中提取局部栅格图，对提取获得的局部栅格图预处理以滤除离散杂点。其中，船当前位置是指当前时刻的船自身的GPS坐标。

[0066] 步骤S15、避障启动判断：检测路线是否存在危险点，若是，启动避障，选取避障目标点，进行避障规划。

[0067] 其中，所述步骤S15后还可包括：当路线不存在危险点时，则路线安全，返回执行步骤S14，以提取下一个局部栅格图，对提取获得的下一个局部栅格图预处理以滤除离散点，以便再次根据新的局部栅格图进行避障启动判断，更全面地进行避障启动判断、避障目标点选取及避障路线规划。

[0068] 步骤S16、避障目标点选取：获取路线的危险点，根据获得的危险点选取避障目标点。

[0069] 步骤S17、避障路线规划：于局部栅格图中根据船当前位置及避障目标点，利用启发式搜索获取避障路线。

[0070] 其中，避障路线是指用于绕开障碍而避免船与障碍发生碰撞的路线，启发式搜索可为Astar算法，所述步骤S17具体包括：

[0071] 于局部栅格图中获取船当前位置及避障目标点；

[0072] 通过代价函数获取起点到船当前位置的实际代价；

[0073] 通过代价函数获取船当前位置到避障目标点的估计代价，以预计船当前位置是否有碰撞危险；

[0074] 通过评估函数获取起点到船当前位置的实际代价与船当前位置到避障目标点的估计代价的和作为通过船当前位置从起点通往避障目标点的总代价值；

[0075] 计算起点周围相邻的栅格点对应的总代价值，选取总代价值最小的栅格点加入至关闭列表；于起点周围相邻的栅格点中选取不在关闭列表且可通过的栅格点作为父节点，将父节点更新为起点，计算更新后的起点周围相邻的栅格点对应的总代价值，选取总代价值最小的栅格点加入至关闭列表；依次循环直至到达避障目标点，从而获取完整的避障路线。

[0076] 所述基于多雷达感知的无人船避障方法通过控制多雷达异步启动，使得多个雷达的收发时间错开，可有效消除多个同频段雷达相互干扰的问题，通过多雷达数据融合扩大感知范围，增强抗干扰能力，可实现多方位的环境感知，并通过获取栅格图的形式规划避障路线，提高避障精度，而且，提取局部栅格图以进行避障规划，可减少计算复杂度，提升避障规划的效率。

[0077] 结合图2，具体地，所述步骤S11中的对获得的各雷达的点云数据打上时间戳并与IMU传感器及GPS传感器的数据进行数据融合，获取各雷达的融合数据包的步骤具体包括：

[0078] 步骤S111、获取各雷达的点云数据，对各雷达的点云数据打上对应的时间戳，根据IMU传感器及GPS传感器的数据获取时间戳对应的时刻船的位姿信息并关联融合至相应的点云数据，获得各雷达的融合数据包。

[0079] 优选地，在本实施例中，所述步骤S111具体包括：

[0080] 获取各雷达的点云数据，根据雷达数据协议对各雷达的点云数据进行解析，将接收到各雷达的点云数据的开头帧数据的时间戳作为该雷达对应的工作周期的起始时间戳，对各雷达的点云数据打上对应的起始时间戳，作为该雷达的对应的工作周期的融合数据包的时间戳。

[0081] 利用IMU传感器获取各起始时间戳对应的时刻的船头朝向，并将对应的船头朝向关联融合至各雷达的下一次工作周期的起始时间戳对应的点云数据，作为各雷达的下一次工作周期的起始时间戳的点云数据的位姿信息。其中，雷达的下一次工作周期的起始时间戳是指雷达在下一个工作周期输出点云数据时，接收到该点云数据的开头帧数据的时间戳。

[0082] 根据各雷达的打上对应的起始时间戳的点云数据及关联融合至该起始时间戳的点云数据的位姿信息、获得各雷达的融合数据包。

[0083] 步骤S112、对各雷达的融合数据包根据对应的时间戳按照时间顺序进行排序，获取各雷达的数据队列。

[0084] 其中，将各工作周期下各雷达的融合数据包的时间戳以便统一时间戳的标准，以避免由于传输方式存在的阻塞或延迟导致的对分析处理结果造成的影响，便于后续同步时间的分析处理而进行时间排序时的准确度，使得数据队列更可靠，感知更准确。优选地，数据队列仅保存定量的各工作周期下的各雷达的融合数据包，存满后，则将数据队列中最早的工作周期对应的各雷达的融合数据包删除以保存新的工作周期对应的各雷达的融合数据包。

[0085] 结合图3，具体地，所述步骤S12包括：

[0086] 步骤S121、对各雷达的融合数据包进行滤波。其中，对各雷达分别进行单独的直通滤波以滤除近处水面杂波及雷达FOV（视场角）外的杂波，提高各雷达的融合数据包的精确度。

[0087] 步骤S122、选取其中一个雷达作为主雷达，剩余的雷达设为子雷达，主雷达的点云数据所在的坐标系作为主坐标系，根据主雷达的融合数据包的时间戳对子雷达的融合数据包进行时间同步。

[0088] 其中，所述步骤S122具体为：选取其中一个雷达作为主雷达，剩余的雷达设为子雷达，主雷达的点云数据所在的坐标系作为主坐标系，主雷达的融合数据包的时间戳作为标准时间戳，根据标准时间戳对各子雷达的数据队列执行出队操作，获取与主雷达在同一工作周期的所有子雷达的融合数据包。

[0089] 步骤S123、将时间同步的所有子雷达的点云数据及主雷达的点云数据合并至主坐标系中，获得融合点云。

[0090] 其中，所述步骤S123具体为：将每个工作周期内与主雷达在同一工作周期的所有子雷达的点云数据及主雷达的点云数据合并至主坐标系中，获得融合点云。通过将主雷达的点云数据包的时间戳作为标准时间戳可便于同步各雷达的时间，通过对同步时间后的各子雷达的点云数据进行旋转平移操作，根据实际安装于船上的各子雷达相对于主雷达的平移向量及偏转角度，计算各子雷达的旋转矩阵及平移向量；并通过计算获得的各子雷达的旋转矩阵及平移向量将各子雷达的点云数据合并至主坐标系中，同时，将主雷达的点云数据亦合并至主坐标系中，从而获得融合点云。

[0091] 步骤S124、使用聚类的方法对融合点云进行滤波处理，滤除融合点云中的离群点，获得点云簇，作为滤波后的雷达融合点云。

[0092] 其中，可采用基于点云密度的聚类方式，以可通过自主设置点云密度的邻域半径及核心点周边点的数量将融合点云聚类形成不同的点云簇，并有利于将无法成类的点云作为离散点云并滤除。

[0093] 结合图4，所述步骤S13具体为：

[0094] 步骤S131、栅格图坐标系建立：以正北为y轴正方向，正东为x轴正方向，并以船自身的初始的GPS坐标作为原点，建立栅格图坐标系。GPS坐标包括船自身的经度和纬度。

[0095] 步骤S132、雷达栅格图获取：根据融合点云的朝向、船自身的GPS坐标及栅格图坐标系的原点，计算出主坐标系相对于栅格图坐标系的旋转矩阵及平移向量，将融合点云投影至栅格图坐标系，获得雷达栅格图。点云的坐标系以船头朝向为y轴正方向，船头朝向即为点云朝向，又船头朝向为位姿信息的yaw角，通过位姿信息的yaw角将点云投影至栅格图坐标系。优选地，栅格图分辨率取1，以使栅格图中的坐标均为整数，去除小数点部分，从而减少计算复杂度，提高计算效率。

[0096] 参照图8，具体地，所述步骤S13前还可包括：

[0097] 步骤S1301、判断是否已存在雷达栅格图。

[0098] 步骤S1302、若是，进行雷达栅格图维护。

[0099] 其中，所述进行雷达栅格图维护的步骤具体包括：

[0100] 根据当前时刻的船头朝向、船自身的GPS坐标及雷达感知范围计算出栅格图维护区域。其中，根据当前时刻的船头朝向和当前时刻的船自身的GPS坐标计算雷达感知范围投影至雷达栅格图的旋转矩阵及平移向量，获取栅格图中的雷达感知范围的区域顶点，获得栅格图维护区域。

[0101] 若栅格图维护区域内的已存在的障碍点在雷达的连续多个工作周期内未被融合点云投影覆盖，取消该栅格为障碍点的标记；若栅格图维护区域内的非障碍点被融合点云投影覆盖，则将该非障碍点更新标记为障碍点。

[0102] 获得维护更新后的雷达栅格图。

[0103] 优选地，所述步骤S1302后可执行步骤S14。所述步骤S1301后还包括：若不存在雷达栅格图，执行步骤S13。

[0104] 参照图1和图5，具体地，所述步骤S14包括：

[0105] 步骤S141、根据当前时刻的船自身的GPS坐标，于栅格图中提取以该GPS坐标为中心的径向尺寸大于雷达探测距离的局部栅格图。通过提取局部栅格图进行分析处理，以便降低后续避障规划的计算复杂度，提高避障规划的效率。

[0106] 步骤S142、对提取获得的局部栅格图进行腐蚀操作，滤除离散杂点。

[0107] 其中，可通过利用3×3卷积核对局部栅格图的像素做“与”操作以进行卷积，实现腐蚀操作，滤除障碍点附近离散的杂点。

[0108] 步骤S143、对滤除离散杂点后的局部栅格图进行卷积膨胀。

[0109] 其中，可通过利用3×3卷积核对局部栅格图的像素做“或”操作以进行卷积，使得障碍点更平滑，有利于降低规划路径的复杂度。

[0110] 结合图6，优选地，所述步骤S15具体包括：

[0111] 步骤S151、于预处理后的局部栅格图上利用路径索引查找船的航行路径的所有路径点。

[0112] 步骤S152、获取与各路径点的沿路径方向的距离大于雷达探测距离的点作为对应的检测点，所有检测点形成局部路线。

[0113] 步骤S153、判断检测点是否为危险点。其中，判断检测点是否为危险点是指判断检测点的危险性，当检测点存在危险时，该检测点为危险点，则路线存在危险点。

[0114] 判断检测点是否存在危险的方式具体为：依序获取以各检测点为中心的呈9×9矩阵形式的栅格作为检测区域，判断对应的检测区域是否存在障碍点；若检测区域存在障碍点，则对应的检测点存在危险，记录该检测点为危险点，当沿路径方向还存在下一检测点时，沿路径方向继续判别下一检测点是否存在危险；若检测区域不存在障碍点，则对应的检测点不存在危险，当沿路径方向还存在下一检测点时，则沿路径方向继续判别下一检测点是否存在危险；遍历所有检测点，当记录有危险点，则局部路线存在危险性，路线存在危险点。

[0115] 步骤S154、若是，启动避障，选取避障目标点，进行避障规划。

[0116] 优选地，所述步骤S153还包括：步骤S155、当检测点不为危险点时，判断是否已遍历所有检测点，若已遍历所有检测点且所有检测点均不为危险点，返回执行步骤S14。所有检测点均不存在危险，则该局部路线安全，此时通过提取下一个局部栅格图，对提取获得的下一个局部栅格图预处理以滤除离散点，以便再次根据新的局部栅格图进行避障启动判断，更全面地进行避障启动判断、避障目标点选取及避障路线规划。

[0117] 结合图7，优选地，所述步骤S16具体包括：

[0118] 步骤S161、获取局部路线的最后一个危险点。

[0119] 步骤S162、判断该局部路线的最后一个危险点是否为全局路线的尾点。

[0120] 步骤S163、若是，选取局部路线的最后一个非危险点的检测点作为避障目标点。

[0121] 步骤S164、若否，于全局路线中沿路径方向在该危险点后的所有路径点中选取距离该危险点预设长度后的第一个非危险点的检测点为避障目标点。

[0122] 综上所述，本发明一种基于多雷达感知的无人船避障方法通过控制多雷达异步启动，使得多个雷达的收发时间错开，可有效消除多个同频段雷达相互干扰的问题，通过多雷达数据融合扩大感知范围，增强抗干扰能力，并通过建立栅格图的形式规划避障路线，提高避障精度，而且，提取局部栅格图以进行避障规划，可减少计算复杂度，提升避障规划的效率。

[0123] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。