CN114879685B - 一种用于无人船的河岸线检测及自主巡航方法 - Google Patents

Abstract

本发明公开了一种用于无人船的河岸线检测方法，包括以下步骤：步骤1、对无人船上的激光雷达采集的数据进行预处理、去噪，得到有效点云数据；步骤2、对雷达点云数据进行网格化，分成N*N网格；步骤3、对每个网格内的点云数据进行处理并提取满足空间特征的空间特征候选点；步骤4、对空间特征候选点进行处理获取满足几何特征的候选点，并对提取的候选点进行校正；步骤5、对步骤4中校正后的候选点进行处理并拟合，得到河岸线曲线，本发明提供了基于多特征融合的方法检测河岸线，包括空间特征：高度差、点云密度特征以及坡度特征，几何特征：距离和角度偏差；通过多特征的融合，能够提高获取的河岸线的精度。

Description

一种用于无人船的河岸线检测及自主巡航方法

技术领域

本发明涉及无人船智能化、自动化领域，具体是一种用于无人船的河岸线检测及自主巡航方法。

背景技术

近年来，无人船艇(USV)技术在执行巡逻、监视、数据收集等任务中发挥着越来越重要的作用。在内陆河道中需要沿岸自主巡航行驶，例如利用无人船沿岸打捞水面垃圾的问题，因为水面漂浮垃圾大多集中河岸边缘，以及利用无人船进行沿岸自动水质监测等。此外，还有通过无人船沿岸自主巡逻需求，在禁止垂钓区域沿岸驱散河边钓鱼人群，在禁止游泳区域沿岸驱散河边游泳戏水人群等。因此，需要提供一种无人船沿岸自主巡航的方法。

无人船在内陆河道自主巡航时，路径规划需要依赖高精度地图，但是高精度地图需要精确测绘，获取成本较高；或者通过人工预设巡航路径点，尤其是在复杂弯曲的河道，需要预先设置弯曲的巡航轨迹，这使得无人船不够智能化。狭窄河道边缘还容易引起无人船搁浅，应与岸边保持一定的安全距离行驶。因此，需要一种低成本的自动化方案，能够安全的自主沿着河岸自动巡航的方法。

专利CN113433933B，《一种智能水面保洁无人船的自主巡航方法》中提出了一种溜边模式，所提出的方法是依靠控制当前船只与岸边的距离来溜边，但是前方河岸边缘具有不确定性，岸边是不规则的，边缘可能会有凸起的障碍，船与岸边的距离可能会突变。其方法仅适用于规则的河道且边缘为直线的情况，没考虑非结构化河道，以及狭窄河道，不能根据河岸形状计算控制优化规划路径，不能根据无人船前方河道边缘曲线形状动态规划出安全行驶路径。

为了保证无人船沿岸自主巡航需要检测河岸边界线，河岸边缘有很多石头，植被，突起的障碍等，使得河岸线检测变得复杂。现有河岸边界检测依赖单一特征，单一特征易受干扰，检测精度难以保证。在河道周围障碍物点云密度较大时或无人船行驶过程中由于受到大风或波浪产生较大俯仰角的情况下，噪声会增加，单一特征提取的方法的鲁棒性会明显降低。因此，需要一种可靠的检测河岸边界线及沿岸巡航的方法。

发明内容

本发明的目的在于提供一种用于无人船的河岸线检测及自主巡航方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种用于无人船的河岸线检测方法，包括以下步骤：

步骤1、对无人船上的激光雷达采集的数据进行预处理、去噪，得到有效点云数据；

步骤2、对雷达有效点云数据进行网格化，分成N*N网格；

步骤3、对每个网格内的点云数据进行处理并提取满足空间特征的空间特征候选点；

步骤4、对空间特征候选点进行处理获取满足几何特征的候选点，并对提取的候选点进行校正；

步骤5、对步骤4中校正后的候选点进行处理并拟合，得到河岸线曲线。

作为本发明进一步的方案：所述步骤1预处理包括去除噪声点以及离群点，然后对点云进行腐蚀、膨胀处理，得到预处理后的点云数据，所述去噪是为了过滤水面干扰，即过滤水面波纹、漂浮物造成的噪声点，所述过滤方法为：

设点云的坐标p_i(x_i，y_i，z_i)，0＜i≤n，过滤后的点云集合Ω为：

Ω＝{q_i(x_i，y_i，z_i)|i∈N∧|z_i-z_min|＞δ}

其中z_min是水面最低点在垂直方向的坐标，q_i是过滤后的点云坐标，N是自然数集合，δ是过滤阈值。

作为本发明进一步的方案：所述步骤3中的空间特征包括点云密度特征、高度差特征、坡度阈值特征。

作为本发明进一步的方案：所述点云密度特征提取方法为：

定义满足点云分布密度条件的候选点集合N^*

其中Δρ_k表示第k个网格C_k与相邻网格点云密度差，l是相邻网格的序号；ρ_cell，k表示k个网格内部点云分布密度系数，N_k是点云个数，S_k是面积大小，T_N1和T_N2是阈值；

所述高度差特征提取方法为：

定义满足高度差条件的候选点集合Z^*

其中ΔZ_k表示第k个网格与相邻网格高度差，是同一河岸边缘候选网格高度差，l是相邻网格的序号，ZStd_k为网格内部高度标准差，n_cell是网格内点云的数量，z_k，j是每个点云的高度z值，

是均值，j是网格内点云ID，而T_z1、T_z2和T_z3是阈值；

所述坡度阈值特征提取方法为：

对于具有固定坡度的河道堤岸，通过设置河道边缘候选格网格(点)与相邻格网格(点)之间的坡度阈值，定义满足坡度条件的候选点集合S^*，坡度通过斜率计算，计算公式如下：

其中，Slope_k是河道边缘候选网格点与相邻格网格点的坡度，(X_i，Y_i，Z_i)为网格内的中位数，T_S1、T_S2是根据河道坡度设定的阈值；

将满足点云密度特征、高度差特征、坡度阈值特征的特征点集合定义为空间特征候选点集：

如果C_k相应特征在阈值范围内，则保留该区域内的点作为河岸边缘候选点，之后，建立一个N×2矩阵，将候选点的二维坐标(x，y)存储为候选网格窗口；如果C_k不满足阈值范围要求，则将该区域设置为空集，即该网格窗口中的点为无效点。

作为本发明进一步的方案：所述步骤4中的满足几何特征的点的提取方法为：

候选点集经过角度排序后，提取的相邻候选点P_i，P_i+1满足角度和距离阈值，边界候选点满足以下条件：

P_launch(X₀，Y₀)是激光雷达发射点的坐标位置，所有光束在此点相交，其中x，y是P(x，y，z)水平范围内点云的坐标，F_dis是相邻候选点和发射点P_launch之间的距离差过滤器，F_ang是相邻候选点和发射点P_launch之间的角度差过滤器，γ_dis和γ_ang是阈值；

相邻三点P_j-1，P_j，P_j+1满足

F_h＝α|y_j-y_j-1|+(1-α)|y_j-y_j+1|＜γ_h

其中F_h是水平距离过滤器，α是公式的权重系数，γ_h是阈值，且0.5＜α＜1；

所述步骤4中对满足几何特征的候选点通过卡尔曼滤波进行校正。

作为本发明进一步的方案：所述步骤5中河岸曲线的拟合采用最小二乘法进行B样条曲线拟合，所述B样条曲线拟合为分段多项式曲线；

其中，c_i(0≤i≤n)是控制点，N_i，l(t)是第i个l次样条基函数；

控制点集通过最小二乘法拟合河岸线候选特征点得到，拟合方程的矩阵形式表示如下：

S＝[H^TH]^-1H^TP

其中S是由控制点集组成的矩阵，H是配置矩阵，P是上一步提取的河岸边界线候选点向量p_k(0≤k≤m)组成的矩阵，m是候选点的个数，t_k表示沿曲线的位置，计算公式如下：

此外，河岸线拟合的样条曲线控制点的数量N_control根据曲线的总长度和节点间距确定，具体计算方法为：

其中L_curve是曲线的总长度，d_n是节点间距，并且N_control是控制点的数量，

表示取最接近的整数。

一种用于无人船的自主巡航方法，包括以下步骤：

S1、根据无人船上激光雷达获取的数据，检测河岸线；

S2、生成河岸线等距线作为沿岸行驶局部路径的规划轨迹；

S3、生成和修正沿岸行驶轨迹的直线和曲线段对应的航迹点；

S4、控制无人船依次沿航迹点平滑过渡行驶。

作为本发明进一步的方案：所述S2中轨迹规划包括宽河道模式和窄河道模式，宽河道则检测单侧河岸线，宽河道设置河道靠边沿岸行驶，窄河道检测左右两侧河岸线，窄河道设置河道中心沿岸行驶；

所述宽河道模式沿岸行驶路径规划为河岸线的等距线，沿岸行驶线可以看作是河道中心线或者河岸边界线的等距曲线，等距曲线方程C_d(t)相应确定如下：

C_d(t)是要规划的行驶线的曲线方程，C(t)＝(x(t)，y(t))表示河岸线的曲线方程参数形式，d为等距距离，是根据不同河道场景设置的规划路径与岸边保持的固定距离；

所述窄河道模式的河道中心线计算包括两种情况，当两侧岸边近似平行和对称时，河道中心线由等距距离d＝L/2时的等距曲线得出，L代表河道的宽度；

当两侧岸边不平行和对称时，沿河道中心行驶时的河道中心线快速近似计算方法为：沿水平方向做扫描线，得到交点再取中点，对所有的中点做曲线拟合，拟合结果作为河道中心线，作为无人船的目标路径轨迹；

具体步骤如下：

L1、沿水平线y＝y_i扫描，与左右边界交点P_l，i(x_l，i，y_i)，P_r，i(x_r，i，y_i)，x_l，i＝f_l(y_i)，x_r，i，＝f_r(y_i)，其中x＝f_l(y)，x＝f_r(y)分别为左右边界曲线方程；

L2、计算P_l，i和P_r，i中点坐标，得到的中点

为河道中心线的点；

L3、令y＝y_i+1＝y_i+Δd，Δd为水平扫描步长，重复步骤1和2继续水平扫描，同理得到中心线采样点P_i+1；

L4、以此类推重复步骤1、2和3，得到连续的河道中心线点P_i、P_i+1...P_i+m；

L5、连接点P_i、P_i+1...P_i+m，并进行三次曲线拟合得到河道中心线，作为目标行驶规划路径。

作为本发明进一步的方案：所述S3中通过对目标行驶路径生成具有直线和曲线段的平滑过渡航迹点，所述平滑过渡航迹点生成方法包括以下步骤：

S3.1、生成保留轨迹特征的连续航迹点P1、P2、...、P_M点，目标跟踪轨迹所需过渡航迹点总数M确定如下：

其中

表示向上取整数，L_track表示跟踪轨迹的总长度，d_gap表示每两个相邻航迹点之间的距离；

S3.2、对航迹点个数进行微调，从起点开始，依次选择三个航迹点P_k(x_k，y_k)、P_k+1(x_k+1，y_k+1)点、P_k+2(x_k+2，y_k+2)点)，计算转向角

S3.3、如果转向角小于阈值，ω＜θ_min，则视为三点共线，认为三点在同一条直线上，删除P_k+1点，转S3.4，否则认为是弧线，保留P_k+1点，转S3.5；

S3.4、如果P_k+2的下一个点P_k+3存在，对Pk点、P_k+2点、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕；

S3.5、如果P_k+2的下一个点P_k+3存在，对P_k+1、P_k+2、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕。

作为本发明进一步的方案：所述无人船通过PID/MVC控制沿规划的航迹点行驶；行驶过程中同时检测障碍物，如遇到障碍物且存在碰撞风险，检测障碍物左侧或者右侧可通行区域，经过可通行区域绕过障碍物，之后快速恢复继续沿岸巡航模式。

与现有技术相比，本发明的有益效果是：

1、本发明提供了基于多特征融合的方法检测河岸线，包括空间特征：高度差、点云密度特征以及坡度特征，几何特征：距离和角度偏差；通过多特征的融合，能够提高获取的河岸线的精度；

2、本发明提供了一种无人船沿岸自主巡航方法，可以适用狭窄河道和宽河道，以及长江、湖泊等宽泛的水域边界；

3、本发提出了一种沿岸行驶路径生成方法，通过计算河道等距曲线生成了水平弯曲的沿岸行驶轨迹，并且对于窄河道提出了一种快速近似计算河道中心线的方法，为河道沿岸安全行驶提供依据；

4、本发提供了一种目标行驶路径航迹点自动生成和修正方法，无人船可以自主沿着河岸保持固定安全距离的直线或曲线行驶。

附图说明

图1为本发明沿岸自主巡航的方法流程图；

图2为本发明河岸线检测及沿岸自主巡航方法详细流程图；

图3为本发明河岸线检测方法流程图；

图4为本发明宽河道靠右沿岸自主巡航时河岸等距曲线目标轨迹规划示意图；

图5为本发明窄河道两侧河岸线平行时河道中心线计算示意图；

图6为本发明窄河道两侧河岸线非平行时河道中心线近似计算示意图；

图7为本发明直线和曲线巡航过渡航迹点生成示例；

图8为本发明窄河道双侧河岸线检测及河道中心等距行驶线生成示例图；

图9为本发明宽河道靠右行驶时单侧河岸线检测及等距行驶线生成示例图。

具体实施方式

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

请参阅图1-9，本发明实施例中，一种用于无人船的河岸线检测方法，本申请采用激光雷达来到面河道边界，本实施方式不限于某一款激光雷达，为了具体描述本方法，我们以无人船搭载Livox Horizon三维激光雷达为例对本方法进行分析。Livox Horizon是一种基于不可重复扫描的机器人激光雷达，以提供比传统激光雷达高得多的分辨率。有效视场角FOV范围为水平81.7°，竖直25.1°，当目标物体的反射率为80％时，FOV的中间位置的最远探测距离为260m左右，FOV的边缘位置，最远探测距离将会相应缩短。为了降低大量的点云数据运算量，通过动态选择感兴趣区域来确定实际需要的激光束，并过滤掉多余的线束。经过以上两步，可以有效减少点云数据量，从而节省后续算法的计算时间。

为了尽可能的减少遮挡和增大激光雷达的探测范围，本实施例中将三维激光雷达安装在无人船船顶位置，并且雷达的旋转轴线与水面垂直。

河岸线边界提取的一般方法是将地面与非地面分离，过滤掉地面点云，然后对障碍物形成的点云进行聚类分析。将点云转换为鸟瞰图BEV，在前景点云提取的基础上，首先利用河道点云的三维空间分布特征，然后通过几何特征提取边界候选点，经过两次过滤而完成河岸边界点的精确提取。首先过滤上层点，高于水面H米的点，将被滤除以减轻计算量，过高的点对无人船不构成障碍，H值由船载LiDAR传感器相对于船的安装高度决定

本实施例的河岸线检测方法包括以下步骤：

步骤1、对无人船上的激光雷达采集的数据进行预处理、去噪，得到有效点云数据；在本实施例中，对激光雷达数据进行预处理、过滤水面噪声干扰，投影到xoy平面保留z值；预处理包括去除噪声点以及离群点，然后对点云进行腐蚀、膨胀处理，得到预处理后的点云数据，所述过滤水面干扰，是为了过滤水面波纹、漂浮物等造成的噪声点，我们采用一种近似的过滤方法，设点云的坐标p_i(x_i，y_i，z_i)，0＜i≤n，过滤后的点云集合Ω为：

Ω＝{q_i(x_i，y_i，z_i)|i∈N∧|z_i-z_min|＞δ}

其中z_min是水面最低点在垂直方向的坐标，q_i是过滤后的点云坐标，N是自然数集合，δ是过滤阈值。

步骤2、对雷达点云数据进行网格化，分成N*N网格；

网格化后网格内的局部点云近似相等，不需要重复计算，这样可以减轻计算量。实施例中算法网格的大小预计设置为10cm*10cm。初始窗口头为待检测的网格窗口，滑动步长为一格。

步骤3、对每个网格内的点云数据进行处理并提取满足空间特征的空间特征候选点，空间特征包括点云密度特征、高度差特征、坡度阈值特征；

点云密度特征提取方法为：定义满足点云分布密度条件的候选点集合N*

其中Δρ_k表示第k个网格C_k与相邻网格点云密度差，l是相邻网格的序号；ρ_cell，k表示k个网格内部点云分布密度系数，N_k是点云个数，S_k是面积大小，T_N1和T_N2是阈值；

高度差特征提取方法为：

定义满足高度差条件的候选点集合Z^*

其中ΔZ_k表示第k个网格与相邻网格高度差，是同一河岸边缘候选网格高度差，l是相邻网格的序号，ZStd_k为网格内部高度标准差，n_cell是网格内点云的数量，z_k，j是每个点云的高度z值，

是均值，j是网格内点云ID，而T_z1、T_z2和T_z3是阈值。

坡度阈值特征提取方法为：

对于具有固定坡度的河道堤岸，通过设置河道边缘候选格网格(点)与相邻格网格(点)之间的坡度阈值。定义满足坡度条件的候选点集合S^*，坡度通过斜率计算，计算公式如下

其中，Slope_k是河道边缘候选网格点与相邻格网格点的坡度，(X_i，Y_i，Z_i)为网格内的中位数，T_S1、T_S2是根据河道坡度设定的阈值。

将满足这三个观察结果的特征点集合定义为空间特征候选点集：

如果C_k相应特征在阈值范围内，则保留该区域内的点作为河岸边缘候选点。之后，建立一个N×2矩阵，将候选点的二维坐标(x，y)存储为候选网格窗口。如果C_k不满足阈值范围要求，则将该区域设置为空集，即该网格窗口中的点为无效点。

需要说明的是计算过程要注意方向，算法计算是在水平方向自左向右，左河岸边界是点云密度突然变小或者高度差突然变小，右河岸边界是点云密度突然变大或者高度差突然变大。所提取的河岸边界点候选点云作为粗略的结果，待下一步精确提取。

空间特征可用于完成河岸边缘点的粗提取，作为边缘候选点。但在河岸边界点周围仍存在一定的噪声，尤其是无人船在前方河道由于波浪导致俯仰角或弯道变化较大时，噪声会增加，容易对河岸边界拟合造成较大干扰。为了进一步提高河岸边界拟合的精度，我们基于上一步搜索得到的候选网格窗口，利用点云几何特征分布的差异，更准确地滤除噪声。

步骤4、对空间特征候选点进行处理获取满足几何特征的候选点；

经过上一步过滤掉高密度点云障碍物后，再对提取的点云进一步进行几何特征过滤，以更准确的提取河岸边界点。

首先，候选点集经过角度排序后，提取的相邻候选点P_i，P_i+1满足角度和距离阈值，边界候选点满足以下条件：

P_launch(X₀，Y₀)是激光雷达发射点的坐标位置，所有光束在此点相交，其中x，y是P(x，y，z)水平范围内点云的坐标，F_dis是相邻候选点和发射点P_launch之间的距离差过滤器，F_ang是相邻候选点和发射点P_launch之间的角度差过滤器，γ_dis和γ_ang是阈值。

其次，相邻三点P_j-1，P_j，P_j+1满足

F_h＝α|y_j-y_j-1|+(1-α)|y_j-y_j+1|＜γ_h

其中F_h是水平距离过滤器，α是公式的权重系数，γ_h是阈值。根据水平方向上的位置信息判断三个相邻点，由于前一个点对边界线的实际检测影响较大，故设置0.5＜α＜1。

通过卡尔曼滤波对步骤4提取的候选点进行校正；

河岸线候选特征点校正或跟踪通过卡尔曼滤波进行。河道边界点检测是基于船身坐标系的，而河岸边界点追踪算法需要将以船体坐标系的边界点坐标都转换为地球坐标系下，可以根据GPS/IMU系统获得地球坐标系下船体的位置信息。基于卡尔曼滤波算法的岸边预测模型为根据时间k时刻的信息，预测时间k+1时刻的状态矩阵和误差协方差矩阵。

x(k)、y(k)分别是河岸的左边界点和河岸右边界点，A(k)、B(k)是状态转移矩阵。Δx(k)、Δy(k)分别是相邻两帧之间在x轴和y轴上的移动距离，φ_k是船体航向角，X(k+1)是预测的下一帧状态，X(k)是当前状态的估计，

是下一状态的误差协方差预测，P(k)是当前状态的误差协方差估计，ω(k)是协方差矩阵为Q的高斯白噪声。当检测到河岸边界点时，认为测量值的噪声为高斯白噪声，测量值向量为：

z(k)＝x(k)+v(k)

v(k)是带有协方差矩阵S的测量值高斯噪声。河岸边界点状态更新(校正)的过程为：

G_k(k+1)是卡尔曼滤波增益，S是测量值噪声的协方差矩阵，X(k+1)是一个河岸边界点的跟踪算法的最终输出，P(k+1)表示当前状态估计的不确定性。河岸两侧的边界点是根据以上公式进行预测和更新(校正)以实现河岸线的跟踪。

步骤5、对步骤4中校正后的候选点进行处理并拟合，得到河岸线曲线，河岸曲线的拟合采用最小二乘法进行B样条曲线拟合，B样条曲线拟合为分段多项式曲线：

其中，c_i(0≤i≤n)是控制点，N_i，l(t)是第i个l次样条基函数；

控制点集通过最小二乘法拟合河岸线候选特征点得到，拟合方程的矩阵形式表示如下：

S＝[H^TH]^-1H^TP

其中S是由控制点集组成的矩阵，H是配置矩阵，P是上一步提取的河岸边界线候选点向量p_k(0≤k≤m)组成的矩阵，m是候选点的个数，t_k表示沿曲线的位置，计算公式如下：

此外，河岸线拟合的样条曲线控制点的数量N_control根据曲线的总长度和节点间距确定，具体计算方法为：

其中L_curve是曲线的总长度，d_n是节点间距，并且N_control是控制点的数量，

表示取最接近的整数，令k＝3，使用三次样条表示河岸线曲线。

一种用于无人船的自主巡航方法，包括以下步骤：

S1、根据无人船上激光雷达获取的数据，检测河岸线；

S2、生成河岸线等距线作为沿岸行驶局部路径的规划轨迹；

S2中轨迹规划包括宽河道模式和窄河道模式，宽河道则检测单侧河岸线，宽河道设置河道靠边沿岸行驶，窄河道检测左右两侧河岸线，窄河道设置河道中心沿岸行驶；

不同类型水面的沿岸巡航有不同的要求，对于开阔水域，如海洋、湖泊，长江和宽河道，沿岸巡逻只要检测单侧水岸边界，无人船靠边沿着单侧边界行驶即可；对于狭窄的河流或运河，原则上河岸决定了可通航区域，需要检测双侧河岸线，沿河道中心线行驶来保证安全，因为河岸线边缘附近的礁石或其他凸出的障碍物会影响USV的航行。单侧河岸线检测与双侧河岸线检测及生成的等距曲线线(沿岸行驶规划轨迹)示例如图8、图9所示；

宽河道和窄河道的判断为当河道宽度L＞d₀认为是宽河道，否则是窄河道。根据宽河道和狭窄河道场景不同，分别设置沿河道靠边行驶，沿河道中心行驶两种不同模式，根据行驶方式生成沿岸行驶线，生成局部路径规划；

沿岸保持d距离行驶，d＞d_min，d_min为无人船需与岸边保持的最小安全距离，d_min根据河道边缘是否规则、是否有障碍，是否易搁浅等因素确定，河道越规则越平整，d_min越小，反之d_min越大；

宽河道模式沿岸行驶路径规划为河岸线的等距线，沿岸行驶线可以看作是河道中心线或者河岸边界线的等距曲线，等距曲线方程C_d(t)相应确定如下：

C_d(t)是要规划的行驶线的曲线方程，C(t)＝(x(t)，y(t))表示河岸线的曲线方程参数形式，d为等距距离，是根据不同河道场景设置的规划路径与岸边保持的固定距离；

窄河道模式的河道中心线计算包括两种情况，当两侧岸边近似平行和对称时，河道中心线由等距距离d＝L/2时的等距曲线得出，L代表河道的宽度；

河道两侧河岸线是否平行和对称根据曲线曲率计算判断，验证两侧河岸线相同中心点的曲率是否近似相等，曲率计算如下：

当两侧岸边不平行和对称时，沿河道中心行驶时的河道中心线快速近似计算方法为：沿水平方向做扫描线，得到交点再取中点，对所有的中点做曲线拟合，拟合结果作为河道中心线，作为无人船的目标路径轨迹；

具体步骤如下：

L1、沿水平线y＝y_i扫描，与左右边界交点P_l，i(x_l，i，y_i)，P_r，i(x_r，i，y_i)，x_l，i＝f_l(y_i)，x_r，i，＝f_r(y_i)，其中x＝f_l(y)，x＝f_r(y)分别为左右边界曲线方程；

L2、计算P_l，i和P_r，i中点坐标，得到的中点

为河道中心线的点；

L3、令y＝y_i+1＝y_i+Δd，Δd为水平扫描步长，重复步骤1和2继续水平扫描，同理得到中心线采样点P_i+1；

L4、以此类推重复步骤1、2和3，得到连续的河道中心线点P_i、P_i+1...P_i+m；

L5、连接点P_i、P_i+1...P_i+m，并进行三次曲线拟合得到河道中心线，作为目标行驶规划路径

S3、生成和修正沿岸行驶轨迹的直线和曲线段对应的航迹点，无人船航迹跟踪通过生成和跟踪一系列巡航过渡航迹点，通过提取等距曲线规划航迹中的航迹点，用作路径跟踪的参考点。在航迹点的生成过程中，权衡航迹点的数量和USV跟踪性能。为了保持路径的连续性特征，实现平滑过渡，沿路径生成尽可能多的航迹点，但是由于过度密集的航迹点会产生大量的控制点，会产生一系列不需要的控制输出，进一步影响USV路径跟踪性能。如果路径上生成的航迹点过少，尤其是转弯圆弧，从直线到圆弧的过渡过程中会发生较大的航向角变化，会产生较大的偏航率，导致不舒适和不经济的自动驾驶。

为权衡航迹点的数量和USV跟踪性能，提出了一种沿岸等距曲线航迹点生成算法，以从计算的沿岸轨迹中获取最佳数量的直线和曲线过渡点，通过航迹点自动生成与调整后，最终直线路径段上所设置的航迹点最少，而圆弧路径段设置适当多的航迹点，便于USV自动控制曲线轨迹跟踪

通过对目标行驶路径生成具有直线和曲线段的平滑过渡航迹点，平滑过渡航迹点生成方法包括以下步骤：

S3.1、生成保留轨迹特征的连续航迹点P1、P2、...、P_M点，目标跟踪轨迹所需过渡航迹点总数M确定如下：

其中

表示向上取整数，L_track表示跟踪轨迹的总长度，d_gap表示每两个相邻航迹点之间的距离；

S3.2、对航迹点个数进行微调，从起点开始，依次选择三个航迹点P_k(x_k，y_k)、P_k+1(x_k+1，y_k+1)点、P_k+2(x_k+2，y_k+2)点)，计算转向角

S3.3、如果转向角小于阈值，ω＜θ_min，则视为三点共线，认为三点在同一条直线上，删除P_k+1点，转S3.4，否则认为是弧线，保留P_k+1点，转S3.5；

S3.4、如果P_k+2的下一个点P_k+3存在，对Pk点、P_k+2点、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕；

S3.5、如果P_k+2的下一个点P_k+3存在，对P_k+1、P_k+2、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕

S4、控制无人船依次沿航迹点平滑过渡行驶，无人船通过PID/MVC控制沿规划的航迹点行驶；行驶过程中同时检测障碍物，如遇到障碍物且存在碰撞风险，检测障碍物左侧或者右侧可通行区域，经过可通行区域绕过障碍物，之后快速恢复继续沿岸巡航模式。

在无人船沿岸自主巡航过程中，目标路径存在障碍物时，计算碰撞风险系数，如果存在碰撞风险，检测障碍物左侧或者右侧的可通行区域，经过可通行区域绕过障碍物后，继续恢复沿岸自主巡航。所述碰撞风险系数(CRI)计算方法如下：

首先，我们按照论文《A Research on Intelligent Obstacle Avoidance forUnmanned Surface Vehicles》方法，根据激光雷达探测的障碍物信息，分别计算第i个障碍物最近会遇距离DCPA_i和最小会遇时间TCPA_i，空间碰撞风险隶属度函数

和时间碰撞风险隶属度函数

d₁表示绝对安全会遇距离，是会遇时需要与对方保持的最小距离；d₂为空间碰撞风险度零边界，是双方安全通过的最小距离；t₁、t₂分别为在紧急情况下避免碰撞的最短和最长时间。

其次，结合时空因素计算第i个障碍物的综合碰撞风险系数CRI_i：

式中μ为时空因素权重系数，0＜μ＜1，当CRI_i大于阈值γ₀，进入避障模式，根据相遇的情况(即会面，交叉，超车)，采取避免碰撞的措施，包括超车、跟随、减速、停泊等多种避碰方案。当CRI小于阈值时，恢复无人船沿岸自主巡航模式。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (8)

1.一种用于无人船的河岸线检测方法，其特征在于，包括以下步骤：

步骤1、对无人船上的激光雷达采集的数据进行预处理、去噪，得到有效点云数据；

步骤2、对雷达有效点云数据进行网格化，分成N*N网格；

步骤3、对每个网格内的有效点云数据进行处理并提取满足空间特征的空间特征候选点；

所述步骤3中的空间特征包括点云密度特征、高度差特征、坡度阈值特征；

所述点云密度特征提取方法为：

定义满足点云分布密度条件的候选点集合N^*

其中Δρ_k表示第k个网格C_k与相邻网格点云密度差，l是相邻网格的序号；ρ_cell,表示k个网格内部点云分布密度系数，N_k是点云个数，S_k是面积大小，T_N1和T_N2是阈值；

所述高度差特征提取方法为：

定义满足高度差条件的候选点集合Z^*

其中ΔZ_k表示第k个网格与相邻网格高度差，是同一河岸边缘候选网格高度差，l是相邻网格的序号，_k为网格内部高度标准差，n_cell是网格内点云的数量，z_k,j是每个点云的高度z值，

是均值，j是网格内点云ID，而T_z1、T_z2和T_z3是阈值；

所述坡度阈值特征提取方法为：

对于具有固定坡度的河道堤岸，通过设置河道边缘候选网格点与相邻网格点之间的坡度阈值，定义满足坡度条件的候选点集合S^*，坡度通过斜率计算，计算公式如下：

其中，Slope_k是河道边缘候选网格点与相邻网格点的坡度，(X_i,Y_i,Z_i)为网格内的中位数，T_S1、T_S2是根据河道坡度设定的阈值；

将满足点云密度特征、高度差特征、坡度阈值特征的特征点集合定义为空间特征候选点集：

如果C_k相应特征在阈值范围内，则保留该区域内的点作为河岸边缘候选点，之后，建立一个N×2矩阵，将候选点的二维坐标(x，y)存储为候选网格窗口；如果C_k不满足阈值范围要求，则将该区域设置为空集，即该网格窗口中的点为无效点；

步骤4、对空间特征候选点进行处理获取满足几何特征的候选点，并对提取的候选点进行校正；

步骤5、对步骤4中校正后的候选点进行处理并拟合，得到河岸线曲线。

2.根据权利要求1所述的一种用于无人船的河岸线检测方法，其特征在于，所述步骤1预处理包括去除噪声点以及离群点，然后对点云进行腐蚀、膨胀处理，得到预处理后的点云数据，所述去噪是为了过滤水面干扰，即过滤水面波纹、漂浮物造成的噪声点，所述过滤方法为：

设点云的坐标p_i(x_i,y_i,z_i),0<i≤n,过滤后的点云集合Ω为：

Ω＝{q_i(x_i,y_i,z_i)|i∈N∧|z_i-z_min|>δ}

其中z_min是水面最低点在垂直方向的坐标，q_i是过滤后的点云坐标，N是自然数集合，δ是过滤阈值。

3.根据权利要求1所述的一种用于无人船的河岸线检测方法，其特征在于，所述步骤4中的满足几何特征的点的提取方法为：

候选点集经过角度排序后，提取的相邻候选点P_i,P_i+1满足角度和距离阈值，边界候选点满足以下条件：

P_launch(X₀,Y₀)是激光雷达发射点的坐标位置，所有光束在此点相交，其中x，y是P(x,y,z)水平范围内点云的坐标，F_dis是相邻候选点和发射点P_launch之间的距离差过滤器，F_ang是相邻候选点和发射点P_launch之间的角度差过滤器，γ_dis和γ_ang是阈值；

相邻三点P_j-1,P_j,P_j+1满足

F_h＝α|y_j-y_j-1|+(1-α)|y_j-y_j+1|<γ_h

其中F_h是水平距离过滤器，α是公式的权重系数，γ_j是阈值，且0.5<α<1；

所述步骤4中对满足几何特征的候选点通过卡尔曼滤波进行校正。

4.根据权利要求1所述的一种用于无人船的河岸线检测及自主巡航方法，其特征在于，所述步骤5中河岸曲线的拟合采用最小二乘法进行B样条曲线拟合，所述B样条曲线拟合为分段多项式曲线；

其中，c_i是控制点，且0≤i≤n，N_i,l(t)是第i个l次样条基函数；

控制点集通过最小二乘法拟合河岸线候选特征点得到，拟合方程的矩阵形式表示如下：

S＝[H^TH]^-1H^TP

其中S是由控制点集组成的矩阵，H是配置矩阵，P是上一步提取的河岸边界线候选点向量p_k组成的矩阵，且0≤k≤m，m是候选点的个数，t_k表示沿曲线的位置，计算公式如下：

此外，河岸线拟合的样条曲线控制点的数量N_control根据曲线的总长度和节点间距确定，具体计算方法为：

其中L_curve是曲线的总长度，d_n是节点间距，并且N_control是控制点的数量，

表示取最接近的整数。

5.使用权利要求1所述河岸线的一种用于无人船的自主巡航方法，其特征在于，包括以下步骤：

S1、根据无人船上激光雷达获取的数据，检测河岸线；

S2、生成河岸线等距线作为沿岸行驶局部路径的规划轨迹；

S3、生成和修正沿岸行驶轨迹的直线和曲线段对应的航迹点；

S4、控制无人船依次沿航迹点平滑过渡行驶。

6.根据权利要求5所述的一种用于无人船的自主巡航方法，其特征在于，所述S2中轨迹规划包括宽河道模式和窄河道模式，宽河道则检测单侧河岸线，宽河道设置河道靠边沿岸行驶，窄河道检测左右两侧河岸线，窄河道设置河道中心沿岸行驶；

所述宽河道模式沿岸行驶路径规划为河岸线的等距线，沿岸行驶线可以看作是河道中心线或者河岸边界线的等距曲线，等距曲线方程C_d()相应确定如下：

C_d()是要规划的行驶线的曲线方程，C(t)＝(x(t),y(t))表示河岸线的曲线方程参数形式，d为等距距离,是根据不同河道场景设置的规划路径与岸边保持的固定距离；

所述窄河道模式的河道中心线计算包括两种情况，当两侧岸边近似平行和对称时，河道中心线由等距距离d＝L/2时的等距曲线得出，L代表河道的宽度；

当两侧岸边不平行和对称时，沿河道中心行驶时的河道中心线快速近似计算方法为：沿水平方向做扫描线，得到交点再取中点，对所有的中点做曲线拟合，拟合结果作为河道中心线，作为无人船的目标路径轨迹；

具体步骤如下：

L1、沿水平线y＝y_i扫描，与左右边界交点P_l,i(x_l,i,y_i)，P_r,i(x_r,i,y_i)，x_l,i＝f_l(y_i)，x_r,i,＝f_r(y_i)，其中x＝f_l(y)，x＝f_r(y)分别为左右边界曲线方程；

L2、计算P_l,i和P_r,i中点坐标，得到的中点

为河道中心线的点；

L3、令y＝y_i+1＝y_i+Δd，Δd为水平扫描步长，重复步骤1和2继续水平扫描，同理得到中心线采样点P_i+1；

L4、以此类推重复步骤1、2和3，得到连续的河道中心线点P_i、P_i+1…P_i+m；

L5、连接点P_i、P_i+1…P_i+m，并进行三次曲线拟合得到河道中心线，作为目标行驶规划路径。

7.根据权利要求5所述的一种用于无人船的自主巡航方法，其特征在于，所述S3中通过对目标行驶路径生成具有直线和曲线段的平滑过渡航迹点，所述平滑过渡航迹点生成方法包括以下步骤：

S3.1、生成保留轨迹特征的连续航迹点P1、P2、…、P_M点，目标跟踪轨迹所需过渡航迹点总数M确定如下：

其中

表示向上取整数，L_track表示跟踪轨迹的总长度，d_gap表示每两个相邻航迹点之间的距离；

S3.2、对航迹点个数进行微调，从起点开始，依次选择三个航迹点P_k(x_k,y_k)、P_k+1(x_k+1,y_k+1)点、P_k+2(x_k+2,y_k+2)点，计算转向角

S3.3、如果转向角小于阈值，ω<θ_min，则视为三点共线，认为三点在同一条直线上，删除P_k+1点，转S3.4，否则认为是弧线，保留P_k+1点，转S3.5；

S3.4、如果P_k+2的下一个点P_k+3存在，对P_k点、P_k+2点、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕；

S3.5、如果P_k+2的下一个点P_k+3存在，对P_k+1、P_k+2、P_k+3三个点，重复S3.2和S3.3，直到所有航迹点执行判断完毕。

8.根据权利要求5所述的一种用于无人船的自主巡航方法，其特征在于，所述无人船通过PID或MVC控制沿规划的航迹点行驶；行驶过程中同时检测障碍物，如遇到障碍物且存在碰撞风险，检测障碍物左侧或者右侧可通行区域，经过可通行区域绕过障碍物，之后快速恢复继续沿岸巡航模式。

Images