0.   引　言

船舶近距离低速靠离泊操作中，对船体运动状态的精确感知至关重要，这直接关系到操作的安全和效率^[1]。然而，传统船舶感知系统在近距离低速环境下对船体运动状态的估计仍存在误差，难以实时获取准确获取船位、靠泊距离、靠泊速度和靠泊角等关键参数^[2]，无法为控制模块提供可靠的数据支持。随着船舶向大型化发展，这种误差会增加操作风险，进一步凸显了提高近距离感知精度的必要性^[3]。

为了减小靠离泊感知误差，目前多采用新的测速、测距装置来优化感知系统^[4]。目前船舶靠离泊感知方法大致可分为两类^[5]：基于岸基的靠离泊感知方法和基于船载传感器的靠离泊感知方法。针对基于岸基的靠离泊感知方法，Perkovic等^[6]提出了激光测距系统，并将该系统用于斯洛文尼亚的科佩尔港，成功测量了大型集装箱船与码头的距离和靠泊速度。Kim等^[7]采用智能视觉监控系统来估计船舶与泊位的相对距离。激光系统具有高精度和快速反应的优势，确保数据的及时性，但一维和二维激光系统难以准确识别目标船舶。为此，闫晓飞等^[8]提出了靠泊信息提取系统，通过3D激光雷达捕捉并构建目标船舶的三维几何形状，从而实现对船舶的靠泊距离、靠泊角和靠泊速度的测量。针对基于船载传感器的靠离泊感知方法，Leite等^[9]利用三维激光技术提取了泊位的几何特征，以估计船舶相对泊位的位置和朝向。Szelangiewicz等^[10]在无人艇（USV）仿真环境中进行了靠泊试验，利用单线激光雷达测定船舶与泊位距离，但忽视了无人艇在横摇、纵摇方向的波动，感知精度在真实海洋环境中可能有所降低。Martinsen等^[11]采用超声波距离传感器测量靠泊距离，以实时计算安全操作区域，该方法可以精确检测无人艇到岸线的距离，但无法实现对泊位距离的检测。在当前研究中，研究人员主要聚焦于对岸线距离检测和三自由度泊位检测，而针对靠离泊过程中从泊位检测到距离感知再到最终的船舶靠离泊定位的研究较少。此外，由于船舶靠离泊过程中感知设备会受到不同程度的干扰^[12]，容易出现测量误差，仅使用单一的感知设备难以准确地感知船舶的位姿。同时，在靠离泊过程中，船体在横摇、纵摇、艏摇方向的波动将导致激光雷达、视频等传感设备的测量结果出现波动。然而，当前的研究中，研究人员往往忽视了对这些波动的补偿，尤其是在横摇和纵摇两个方向。

为此，本文将采用一种融合激光雷达和惯性导航的船舶靠离泊定位方法，以实现高精度定位。首先，研究不同感知设备在时间和空间上的统一算法，利用惯导数据修正激光点云的测量波动，实现点云“防抖”；其次，提出一种改进的随机采样一致性算法（random sample consensus，RANSAC），用于检测泊位岸线，并将识别的岸线交集作为泊位的特征点，辅助船舶进行定位；最后，在相关大型无人艇靠离泊感知实验中对所提算法进行验证。

1.   问题描述与相关理论

本文主要解决船舶靠离泊时的泊位识别和定位问题，按照不同参考坐标系可以将该问题分为两种类型^[13]：一是船舶在某一固定坐标系下的定位问题，二是船舶附体坐标系下障碍物识别与定位的问题。这两种类型的问题可以互相通过坐标映射的方式实现多传感器的数据融合。

1.1   导航坐标系定义

本文涉及的坐标系包括5种：地球惯性坐标系（earth-centered inertial frame，ECI）${{O}_{\mathrm{i}}-x}_{\mathrm{i}}{y}_{\mathrm{i}}{{\textit{z}}}_{\mathrm{i}}$、北东地坐标系（north-east-down frame，NED）${O}_{\mathrm{n}}-{x}_{\mathrm{n}}{y}_{\mathrm{n}}{{\textit{z}}}_{\mathrm{n}}$、附体坐标系（body-fixed frame，BF）${{O}_{\mathrm{b}}-x}_{\mathrm{b}}{y}_{\mathrm{b}}{{\textit{z}}}_{\mathrm{b}}$、激光雷达构建地图坐标系^[14]（LiDAR mapping frame，LR）${{O}_{\mathrm{l}\mathrm{r}}-x}_{\mathrm{l}\mathrm{r}}{y}_{\mathrm{l}\mathrm{r}}{{\textit{z}}}_{\mathrm{l}\mathrm{r}}$和局部坐标系（local coordinate frame，LC）${{O}_{\mathrm{c}}-x}_{\mathrm{c}}{y}_{\mathrm{c}}{{\textit{z}}}_{\mathrm{c}}$，具体定义方式如图1所示。图中，局部坐标系选用船舶开始离泊时的激光雷达位置为原点，${x}_{\mathrm{c}}$轴、${y}_{\mathrm{c}}$轴、${{\textit{z}}}_{\mathrm{c}}$轴的方向分别是平行、垂直于岸线的长边和竖直向上。

图  1  坐标系定义

Figure  1.  Coordinate systems definition

下载: 全尺寸图片 幻灯片

船舶可通过全球导航卫星系统（GNSS）获取自身在地球惯性坐标系的位置，将ECI坐标系转换为NED坐标系。若忽视高度信息，则可以完全转换为NED坐标系，具体方法在此不予赘述，本文最后的结果是在局部坐标系中呈现。

船端的感知设备，例如惯性测量单元（inertial measurement unit，IMU）所测得的数据是基于船舶的附体坐标系。在附体坐标系中，坐标轴${x}_{\mathrm{b}}$指向船首，坐标轴${y}_{\mathrm{b}}$指向右舷，坐标轴${{\textit{z}}}_{\mathrm{b}}$指向龙骨。IMU测量船舶在这3个坐标轴的位移和旋转。在进行定位时，需要先将船舶在附体坐标系下的位移和旋转转换到NED坐标系下。

激光雷达在测量时，也有自身的地图坐标系，这一坐标系与船舶的附体坐标系类似，坐标轴${x}_{\mathrm{l}\mathrm{r}}$指向雷达正前方，坐标轴${y}_{\mathrm{l}\mathrm{r}}$指向雷达正右边，坐标轴${{\textit{z}}}_{\mathrm{l}\mathrm{r}}$指向正下方。在已知激光雷达在船舶的安装位置和安装角度的前提下，坐标系$O\mathrm{_{lr}}-x\mathrm{_{lr}}y_{\mathrm{lr}}\text{z}_{\mathrm{lr}}$可以通过旋转、平移的方式转换到坐标系${{O}_{\mathrm{b}}-x}_{\mathrm{b}}{y}_{\mathrm{b}}{{\textit{z}}}_{\mathrm{b}}$，这样可以将激光雷达的点云数据转换到附体坐标系中。为方便绘图，本文的显示坐标系均为局部坐标系，但数据的处理、空间转换的基准坐标系是NED坐标系。

1.2   导航坐标转换

本文主要以NED坐标系作为展示与结果分析的坐标系，其余坐标系均需要转换到NED坐标系下。NED坐标系的原点设定为初始状态下船舶的位置，以此为参考完成地球惯性坐标系与NED坐标系之间的相互转换。

船舶附体坐标系中${x}_{\mathrm{b}},{y}_{\mathrm{b}},{{\textit{z}}}_{\mathrm{b}}$三轴的速度分量和旋转角度为$\{u,v,w,\varphi ,\theta ,\psi \}$，该信息由IMU测得。船舶附体坐标系中的运动可通过以下公式转换为NED坐标系下的运动：

式中：${\boldsymbol{R}}_{{\textit{z}}}\left(\psi \right)$，${\boldsymbol{R}}_{y}\left(\theta \right)$，${\boldsymbol{R}}_{x}\left(\varphi \right)$分别为绕${\textit{z}}， {\textit{y}}， {\textit{x}}$三轴旋转$\psi ,\theta ,\varphi$度的旋转矩阵；${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{n}}\left(\psi ,\theta ,\varphi \right)$为附体坐标系旋转到NED坐标系的欧拉角旋转矩阵；${\boldsymbol{P}}_{\mathrm{n}}\left(k+1\right)$为NED坐标系下船舶在$k+1$时刻的位置；h为采样时间；Θ_b（k）为k时刻船舶的三轴姿态角；${\boldsymbol{\nu }}_{\mathrm{b}}\left(k\right)=\{u,v,w\}$为k时刻船舶三轴的速度分量。

激光雷达与IMU的安装位置和角度可能存在不可避免的偏差，因此需要将点云坐标系的数据转换到船舶附体坐标系，然后通过IMU数据将其转换到NED坐标系。点云坐标系到附体坐标系的处理方式与附体坐标系到NED坐标系类似。定义点云坐标系旋转到附体坐标系的欧拉角旋转矩阵为${\boldsymbol{R}}_{\mathrm{l}\mathrm{r}}^{\mathrm{b}}\left(\psi ,\theta ,\varphi \right)$；其次，因为激光雷达与船舶相对静止，仅需要建立一个平移矩阵${\boldsymbol{T}}_{\mathrm{l}\mathrm{r}}^{\mathrm{b}}\left(X,Y,Z\right)$来修正这一偏移量。激光点云在k时刻时，其中一点的坐标为${\boldsymbol{P}}_{\mathrm{l}}\left(k\right)$，可由式(6)计算。

式中，${\boldsymbol{P}}_{\mathrm{b}}\left(k\right)$为该点在船舶附体坐标系中的位置，通过附体坐标系到NED坐标系的转换。激光雷达生成的点云坐标将被转换至NED坐标系中，如图2所示。

图  2  北东地坐标系下的点云图

Figure  2.  Point cloud in NED coordinate system

下载: 全尺寸图片 幻灯片

1.3   硬件参数与时间配准

通过上述工作，原始激光点云数据已被转换为NED坐标系下点云数据格式，下一步需对靠离泊泊位等进行识别并定位。船舶靠离泊定位只需基于二维平面进行计算分析。为便于描述，本文选择一艘船长45 m、船宽12 m的大型无人艇作为研究对象进行说明，该无人艇以船载的激光雷达安装点作为船体固定点。以泊位内凹点作为泊位特征点，如图3所示。其中，激光雷达为16线，采样频率为10 Hz。IMU和GNSS的采样频率分别为50 和10 Hz。通过识别泊位的岸线以及各岸线的交会处，可实现对泊位特征点的定位。由于各传感器的采样频率存在差异，需要对激光点云进行时间上的配准。选择10 Hz作为处理的基准频率，并对IMU的数据进行相应的采样调整，从而确保各传感器的数据在时间上保持一致。

图  3  靠离泊点云特征点提取

Figure  3.  Feature point extraction of berthing and unberthing point cloud

下载: 全尺寸图片 幻灯片

2.   船舶靠离泊感知数据处理

单一传感器难以满足船舶靠离泊信息感知的要求。例如，GNSS定位虽然存在一定经纬度漂移问题，但其在距离泊位较远的地方仍然能保证定位精度；激光雷达定位精度高，但在靠离泊这种复杂场景下，其与泊位距离较远时，泊位的点云数量会下降，从而降低了定位精度。因此，船舶靠离泊感知有必要使用多传感器数据进行融合互补。

2.1   点云预处理

原始的三维点云数据需要转化为泊位特征点。首先，对点云数据进行预处理，包括点云数据格式转换、坐标转换等，其中坐标转换已在1.2节中进行了描述。

其次，为实现船舶的靠离泊定位，还需对点云数据去噪处理。本文采用统计滤波算法来剔除点云中的噪声点数据，激光点云滤波效果如图4所示，其中噪声点数据的来源主要包括扫描得到的泊位附近的固定设施点云数据和水面波动产生的噪声点，此处对点云数据进行统计分析，以识别出水面波动所产生的噪声点数据并将其滤除；而固定设施噪声点一般数据量较大，去除起来比较困难，本文通过所提改进的RANSAC算法识别出岸线，设置阈值从而剔除该噪声点数据。

图  4  激光点云滤波效果

Figure  4.  Laser point cloud filtering effect

下载: 全尺寸图片 幻灯片

2.2   基于改进的RANSAC算法岸线提取

2.2.1   改进的RANSAC算法

本文采用RANSAC算法的直线拟合模型来拟合岸线。该算法通过随机采样一小部分数据点来估计模型参数，并使用这个估计的模型对整个数据集进行验证。RANSAC算法具有鲁棒性强、适用于参数化模型、阈值可调整的特点，能够在存在大量离群点的情况下找到最佳拟合模型^[15]。在靠离泊场景中往往存在水面波动或潮汐等因素导致的离群点，岸线也可以用参数化模型来表示。因此，本文采用RANSAC算法来拟合岸线。但若将RANSAC算法直接用于岸线提取，将会面临以下问题：1）要合理确定算法中输入的距离阈值，否则拟合的效果会较差^[16]；2）由于在迭代过程中，该步骤的成本函数是符合约束的内点的个数，若用于提取岸线，岸基建筑点云和噪声样本点云可能被视为内点，从而增加内点数量，造成误差，降低性能。

为了验证上述问题，设计了一个实验。所设计的样本符合$y=2x$线性模型，同时引入环境噪声作为干扰值。选择不同的距离阈值来拟合直线。其中，图5（a）展示了阈值为1 m时的拟合结果，图5（b）和图5（c）是阈值为50 m时的拟合结果。经过对比发现，在图5（b）中，样本的拟合是不准确的；相反，在合适的阈值1 m下，样本的拟合更为准确。此外，图5（b）和图5（c）是距离阈值均设置为50 m时的拟合结果，出现两种不同的拟合结果是因为RANSAC算法在搜索内点时存在随机性，以及筛选最优模型的成本函数不合适。

图  5  RANSAC算法在不同距离阈值下的拟合结果

Figure  5.  Fitting results of RANSAC algorithm under different distance thresholds

下载: 全尺寸图片 幻灯片

因此，本文提出一种基于RANSAC的新型岸线提取算法，以便更精确地提取岸线，更好地适用于靠离泊的各种场景。在本研究中，主要的检测目标为泊位，因此参照文献[16]中的方式，将泊位的高程（露出水面的高度）作为距离阈值的设定依据，如图6所示。其中，受到涨落潮的影响，泊位的高程范围为$\left({h}_{1},{h}_{2}\right)$，激光雷达安装的高程为${h}_{3}$。取涨潮时泊位高程${h}_{1}$为距离阈值，保证RANSAC聚类的点云都处于泊位区域。

图  6  激光雷达靠离泊测量示意图

Figure  6.  Schematic of LiDAR berthing and unberthing measurement

下载: 全尺寸图片 幻灯片

此外，还提出了一种考虑岸基到船载激光雷达距离特征的成本函数，如式(7)所示，不再使用符合约束的内点个数来筛选最优模型，而是将符合约束的内点继续进行分类，不同类别的内点对成本函数有不同的影响比重。在此基础上，可以减少随机性的影响，使算法效果更好^[17]。

式中：C为总成本；N为点云的数量；${P}_{i}$为第i个点的坐标；L为目标直线；$d({P}_{i},L)$为点到目标直线的距离；$\sigma$为衰减函数的标准差，控制成本函数的衰减速度。

图7所示为改进后的RANSAC算法流程。通过估计最优距离阈值并设计更适用于靠离泊场景的成本函数，减小噪声点云对结果的影响。在岸线提取阶段，对三维激光点云进行处理。基于对实验所采集数据的观测，将最小内点的个数设定为50，迭代次数设定为500。为了检测泊位的多条岸线，对每次运行RANSAC算法的包含点云集进行剔除，然后在剩余点云中继续运行算法，以找到所有的泊位直线点云合集。在识别所有岸线后，计算所识别岸线之间的交点，并选择距离局部坐标系原点更近的点作为特征点。通过坐标转换，可获得特征点在各个坐标系下的位置信息。

图  7  改进 RANSAC 算法流程图

Figure  7.  Flowchart of the improved RANSAC algorithm

下载: 全尺寸图片 幻灯片

2.2.2   岸线提取效果分析

本节主要测试改进RANSAC算法在岸线点云数据较稀疏、较稠密以及岸基建筑噪声点较多等不同情况下的提取效果，并与常规RANSAC算法进行比较，以验证本文所提方法的有效性和适用性。

在靠离泊场景中，本文提出的改进RANSAC算法对岸线的识别效果如图8所示。其中，聚类后的点云集和聚类生成的岸线最优模型将被用于后续的靠离泊定位中。

图  8  基于改进RANSAC算法的点云岸线识别结果

Figure  8.  Point cloud shoreline recognition results based on improved RANSAC algorithm

下载: 全尺寸图片 幻灯片

常规RANSAC和改进RANSAC算法在离泊过程中的岸线提取结果对比如图9所示。其中，图9（a）和图9（b）分别表示在场景1和场景2下，无人艇开始离泊时的岸线提取效果对比。图9（c）和图9（d）分别表示在场景3和场景4下，离泊过程中的岸线提取效果对比。图中，灰色点为激光雷达获取的点云数据，红线和绿线分别为使用常规RANSAC算法和改进RANSAC算法的提取结果。不同场景下提取岸线与实际岸线的偏差角如表1所示。

图  9  离泊过程岸线提取结果对比图

Figure  9.  Comparison of shoreline extraction results during unberthing process

下载: 全尺寸图片 幻灯片

表  1  不同方法提取岸线与实际岸线的偏差角

Table  1.  Deviation angles between extracted shorelines and actual shoreline by different methods

场景	点云密度	常规RACSAC 岸线偏差角	改进RACSAC 岸线偏差角
场景1	稠密	9°48′	1°7′
场景2	稠密	3°39′	1°10′
场景3	稀疏	29°19′	1°13′
场景4	稀疏	6°31′	0°56′

下载: 导出CSV 

| 显示表格

当无人艇开始离泊时，运用常规RANSAC算法所提取的岸线与实际岸线的偏差角为3°39′，改进RANSAC算法的偏差角为1°10′。此时，两种算法的提取精度比较接近，这是因为船舶距离泊位较近，激光雷达采集到的泊位点云数据比较完整。当无人艇开始运动时，运用常规RANSAC算法所提取的岸线和实际岸线的偏差角为29°19′，而改进RANSAC算法的偏差角仅为1°13′。两种结果相差较大，改进RANSAC算法的提取精度明显优于常规RANSAC算法。这是由于激光雷达可能受到距离、船舶姿态变化的影响，采集到的岸线点云数据不完整且数量较少，而且常规算法以符合距离阈值约束的内点个数作为选择模型的成本函数，会使激光雷达扫描到的岸基建筑的点云数据对拟合岸线的结果造成影响^[18]，导致误差较大。在激光雷达采集数据较稀疏的情况下，改进RANSAC算法表现出较明显的优势；当点云数据较稠密时，改进RANSAC算法仍有较好效果。

3.   实验验证

本文的实验验证采用了于2023年5月15日在某海域采集的船长45 m、船宽12 m的无人艇离泊实验数据，靠离泊测试场景如图10所示。

图  10  靠离泊测试场景

Figure  10.  Berthing and unberthing test scenario

下载: 全尺寸图片 幻灯片

当无人艇远离泊位时，激光雷达点云数据过于稀疏，在点云识别层面的精度已经无法保障。因此，实验选取了无人艇离泊过程中的前800帧（80 s，4000条IMU数据）激光点云作为数据测试案例。

无人艇的三轴角度变化如图11所示。无人艇的运动主要在艏摇方向，而在另外两轴的波动较小。无人艇的轨迹如图12所示，坐标原点为实验开始时刻无人艇的位置。其中IMU生成的无人艇轨迹是由IMU东速北速积分而得。GNSS数据是由实时更新的定位信息与实验起始时刻的定位信息通过经纬度到平面的坐标转换得出。由图12可以看出，GNSS数据存在一定波动，而且GNSS数据和IMU数据之间存在相位偏移，这是由于IMU和GNSS的安装位置不同、无人艇转向时的转心也不同。

图  11  靠离泊过程中无人艇三轴变化情况

Figure  11.  Changes in three axes of USV during berthing or unberthing process

下载: 全尺寸图片 幻灯片

图  12  靠离泊过程GNSS和IMU的无人艇轨迹

Figure  12.  Trajectory of USV during berthing and unberthing using GNSS and IMU

下载: 全尺寸图片 幻灯片

通过将IMU数据作为激光雷达的姿态补偿，整个离泊过程可被映射到NED坐标系中（图13）。图13（a）为离泊起始时刻的点云，图13（b）为离泊结束时刻的点云。在离泊过程中，无人艇的运动轨迹、开始和结束时刻点云以及这两个时刻特征点在局部坐标系中的位置如图14所示。由图可以观察到特征点定位存在漂移的情况。这一漂移误差是因IMU对激光雷达位移补偿的偏差所致，IMU与激光雷达的安装位置不同，其在空间上的运动轨迹也不相同，所以此误差难以避免。

图  13  实验过程点云变化图

Figure  13.  Point cloud changes during experimental process

下载: 全尺寸图片 幻灯片

图  14  无人船离泊过程的运动轨迹与开始、结束时刻的特征点位置

Figure  14.  Motion trajectory of USV during unberthing process and feature point positions at start and end moments

下载: 全尺寸图片 幻灯片

值得注意的是，传统RANSAC算法造成的误差已经通过2.2节使用的方法得以消除，利用激光雷达定位具有一定的可靠性，但在距离较远、点云稀疏时，激光雷达的定位误差仍然会逐渐增大。本文在前期对检测的特征点进行了精确的经纬度坐标标定，通过计算无人艇GNSS实时定位数据与特征点坐标经纬度，可以获取GNSS定位距离。同理，基于IMU中东速、北速的积分，可以获得更加精确的无人艇到特征点的距离信息。经验证，实验所用的IMU在1 min内的测量误差约为0.056 m，明显小于GNSS方法以及激光雷达融合算法的值。因此，可将这一基于IMU的距离作为测量基准值（图12）。采用不同方法测量的无人艇到岸线特征点的距离变化如图15所示。其中，3条线分别代表融合激光雷达和IMU两种传感器所得到的距离变化曲线、使用单个GNSS传感器定位得到的距离变化曲线，以及距离变化基准值曲线。计算得出，本文方法的平均测量误差为0.256 m，最大测量误差为0.596 m。而GNSS测量结果的平均误差为0.478 m，最大测量误差为0.839 m。本文方法的精度相比于GNSS提升了46.4%。此外，采用 GNSS 方法进行靠离泊定位还需要先确定泊位特征点的高精度经纬度坐标，而本文所提方法只需要预先估算泊位的高程，即可获得更加精确的定位信息。文献[18]所采用的岸基测量方法可以提供平均误差为0.810 m的测距信息，与之相比，本文所采用的船载方案可以提升约68.4%的精度。值得注意的是，图15所示激光雷达和GNSS曲线与距离变化基准值曲线始终存在一定偏移，这是由于无人艇上的雷达、IMU和GNSS 天线的安装位置有所不同所致。

图  15  不同定位方法到特征点的距离对比

Figure  15.  Distances from different positioning methods to feature point

下载: 全尺寸图片 幻灯片

4.   结　语

本文融合激光雷达、IMU、GNSS感知数据对无人艇靠离泊高精度定位问题进行了研究。在对各感知设备数据进行时空配准基础上，提出了一种基于改进RANSAC的岸线提取方法，将多岸线重合的交点作为泊位的特征点，实现船舶在坐标系中的定位，并提高了检测岸线的精度。实验结果表明，本文提出的改进RANSAC算法能够为船舶提供误差在0.256 m以内的可靠定位信息，具有更强的稳定性和可靠性。该方法能发挥激光雷达测距精度高的优势，为船舶靠离泊定位相关研究提供参考。

但是，本文方法也存在一定的局限性。在硬件层面，其受到激光雷达的视场角与线程约束。以本研究所采用的16线激光雷达为例，其对泊位感知的有效距离约为30 m。超过这一距离后，激光点云会变得极为稀疏，无法稳定探测到泊位。在算法层面，本文方法需要预估泊位高程，在泊位高程较小的情况下，算法的识别精度可能降低。

在未来的研究中，可采用更高性能的激光雷达设备，或者采用视觉信息提前识别泊位的方位、高程的参考信息，提升算法对泊位的识别精度。此外，在后续靠离泊感知问题中，可考虑采用远程、近程传感器设备，获取完整流程的靠离泊定位信息。利用激光雷达、毫米波雷达等近距离传感器获取精度较高的感知定位信息，同时结合远距离探测设备以降低成本。通过多种设备的交替使用，实现全流程的靠离泊感知。