0 引　言

水面无人艇（Unmanned Surface Vehicle，USV）自21世纪初起，因其在安全性、减少人员风险、机动性、隐蔽性和低运营成本中的优势，成为海洋强国战略的重要组成部分。随着其在海洋监测、搜救和军事任务中的广泛应用，对自主导航系统的性能要求日益严格。激光SLAM（Simultaneous Localization and Mapping）技术，尤其是3D版本，是实现USV自主导航的关键，它能实时构建环境地图，支持USV自主靠离泊等操作。

近年来，激光SLAM算法，研究方向分为松耦合（如LOAM^[1]）和紧耦合（如LIO-Mapping^[2]、R-LINS^[3]和LIO-SAM^[4]）方法。尽管松耦合如LOAM依赖环境特征，紧耦合如LIO-SAM追求实时性，但针对水面环境的研究仍较少。

闭雄栅等^[5]提出基于粒子滤波的Fast-SLAM，初步应用于水面无人船定位，但未整合IMU（Inertial Measurement Unit）数据，影响复杂环境下的鲁棒性。S4-SLAM^[6]在2021年发布，通过结合改进的Super4PCS（Super Four Point Congruent Sets）算法和迭代最近点（Iterative Closest Point，ICP）方法提高了位置更新频率，但IMU和GPS信息融合不足影响精度。李可染等^[7]提出基于岸堤特征的近岸无人船激光SLAM，虽能减少里程计漂移，但对水波干扰的抑制仍有待加强。

综上所述，传统激光SLAM主要针对陆地，USV靠离泊环境下的挑战包括：动态水面导致IMU误差增加，水波干扰影响数据质量，以及在资源有限的无人艇上，局部极值问题影响算法性能^[8]。为了克服这些困难，提出一种对LIO-SAM框架进行改进的USV激光SLAM算法，旨在提升USV激光SLAM在泊位环境中的稳定性和准确性。

1 改进算法设计 1.1 算法整体框架

针对USV在自主靠离泊环境中的特定挑战，本文提出一种改进的LIO-SAM算法。LIO-SAM算法是经典的基于平滑和建图的紧耦合激光-惯性里程计，算法里程计部分延续LOAM的思想，提取特征点，并使用IMU数据校正点云畸变，提供数据帧之间位姿变换的初始值；后端采用因子图优化架构，消除累计误差，进行全局优化。改进后算法的整体框架分为点云预处理、IMU预积分、回环检测和因子图优化4个模块，如图1所示。其中，点云去畸变模块作为框架的入口，负责对激光雷达数据中的运动畸变进行校正，并输出校正后的点云数据。

IMU预积分模块通过对IMU原始测量数据做积分处理，为每帧点云估计初始位姿，该模块也为关键帧的选取提供依据。因子图优化模块利用4种因子，最小化这些约束关系的误差，得到全局一致且精确的状态估计。

1.2 点云去畸变

在USV的激光SLAM中，激光雷达的运动导致采集的点云出现畸变。问题源于激光雷达的工作原理：数据帧通过整个扫描周期内累积点云形成，而非即时数据快照。在此过程中，雷达可能会经历旋转和平移等运动，引起点云原点的不一致性，从而产生运动畸变。如图2所示，这是其形成的关键机制。

点云去畸变旨在将所有数据补偿到起始时刻。畸变校准方法包括传感器辅助法和纯估计法。传感器辅助法利用高频里程计和IMU测量数据计算点云相对于起始时刻的位姿；而纯估计法基于匀速运动模型^[3]，但无人艇运动的不稳定性使其效果不佳。本研究采用传感器辅助法，通过IMU数据计算点云运动变化，实现畸变校正。

首先进行坐标转换。由于无人艇受风浪流影响，激光雷达与水平面之间存在动态变化倾角，为了提升特征点提取的准确度，本研究利用IMU提供的横滚角ϕ和俯仰角ψ，将激光雷达坐标系的点云转换至与水平面平行的坐标系^[7]，转换过程如下式：

$ {\left[ \begin{array}{*{20}{c}} x \\ y \\ z\end{array} \right] = \left[ \begin{array}{*{20}{c}}\cos(\psi) & \sin(\psi)\sin(\varphi) & \sin(\psi)\cos(\varphi) \\ 0 & \cos(\varphi) & -\sin(\varphi) \\ -\sin(\psi) & \cos(\psi)\sin(\varphi) & \cos(\psi)\cos(\varphi)\end{array} \right] \left[ \begin{array}{*{20}{c}}x_L \\ y_L \\ z_L \end{array} \right]}。$

(1)

点云中每个回波点的坐标值[x_L,y_L,z_L]在雷达坐标系L下被转换为新坐标系下的坐标值[x, y, z]。

接着进行畸变补偿。假设在离散的时间间隔[t_i,t_i+1]内，IMU测得的角速度为$ \omega (t) $和线速度为$ v(t) $。为了计算在这个时间间隔内无人艇的位姿变化，采用以下方式进行位姿预积分：角速度$ \omega (t) $可以通过指数映射转换为旋转矩阵的增量，表达式为：

$ \Delta R({t_{i + 1}},{t_i}) = \exp (\displaystyle\int_{{t_i}}^{{t_{i + 1}}} {\hat \omega } (t){\mathrm{d}}t。$

(2)

式中：$ \hat \omega (t) $为角速度的反对称矩阵形式，用于转换为李代数so(3)上的元素。随后对线速度$ v(t) $进行积分以计算速度变化$ \Delta v({t_{i + 1}},{t_i}) $和位置变化$ \Delta p({t_{i + 1}},{t_i}) $：

$ \left\{ \begin{gathered} \Delta v({t_{i + 1}},{t_i}) = \int_{{t_i}}^{{t_{i + 1}}} {v(t)} {\mathrm{d}}t ，\\ \Delta p({t_{i + 1}},{t_i}) = \int_{{t_i}}^{{t_{i + 1}}} {\Delta v(t){\mathrm{d}}t}。\\ \end{gathered} \right. $

(3)

为了提高积分的准确性，这里采用了梯形积分法，该方法通过在时间间隔的两端取平均值来近似积分。根据位姿预积分的结果，可以计算在时间t采集的点云中点$ P(t) $运动补偿后的位置$ D' $：

$ {P'(t) = \Delta R{({t_{i + 1}},t)^{\rm{T}}} \cdot (P(t) - \Delta p({t_{i + 1}},t)) - \Delta v({t_{i + 1}},t) \cdot (t - {t_i})。} $

(4)

式中：$ P'(t) $为补偿后的点位置。

通过上述方法，能够有效地对无人艇在采集点云数据过程中的运动造成的畸变进行补偿，从而提高SLAM系统的精度和鲁棒性。

1.3 IMU预积分优化

在上述点云去畸变过程中，IMU预积分是通过对连续的IMU测量值进行积分来估计2个关键帧之间的位姿变化。然而，在水面高动态变化场景下，IMU的测量噪声和偏差会导致预积分误差增大，影响最终的定位精度。为了解决这个问题，本文对IMU预积分过程进行以下优化，算法流程图如图3所示。

1）采用滑动平均技术优化了IMU数据的平滑处理。为了精确估计噪声，为后续计算做好准备，在固定时间窗口W内，收集IMU的测量值，然后计算平均值和协方差。对于角速度和加速度数据，噪声参数的估计分为两步。

①计算无噪声情况下的参考值，即角速度和加速度的均值：

$ \left\{ \begin{gathered} \bar \omega = \frac{1}{W}\sum\limits_{k = 1}^W {{\omega _k}}，\\ \bar a = \frac{1}{W}\sum\limits_{k = 1}^W {{a_k}}。\\ \end{gathered} \right. $

(5)

②计算它们的协方差矩阵，以此定量描述噪声的强度：

$ \left\{\begin{gathered}n_{\omega}=\frac{1}{W-1}\sum\limits_{k=1}^W(\omega_k-\overline{\omega})(\omega_k-\overline{\omega})^{\rm{T}}，\\ n_a=\frac{1}{W-1}\sum\limits_{k=1}^W(a_k-\overline{a})(a_k-\overline{a})^{\rm{T}}。\\ \end{gathered}\right. $

(6)

2）引入了一种自适应的预积分时间间隔方法。它能根据载体平台的动态姿态变化进行调整，快速变化时，时间间隔减小以提高精度；姿态稳定时，时间间隔增大以减少计算负担，这种策略在保证预积分准确性的同时，优化了计算效率。通过定义一个动态阈值$ \tau $，依据当前运动状态和噪声水平，来平衡积分误差和计算复杂度。

在先前的计算中，已经确定了噪声的特征参数，接下来分别计算加速度和角速度变化量的方差，以此来描绘运动状态的特性：

$ \left\{ \begin{gathered} \sigma _{\vartriangle a}^2 = \frac{1}{{W - 1}}\sum\limits_{k = 1}^{W - 1} {{{({a_{k + 1}} - {a_k})}^2}} ，\\ \sigma _{\vartriangle \omega }^2 = \frac{1}{{W - 1}}\sum\limits_{k = 1}^{W - 1} {{{({\omega _{k + 1}} - {\omega _k})}^2}} 。\\ \end{gathered} \right. $

(7)

这里$ \sigma _{\vartriangle a}^2 $和$ \sigma _{\vartriangle \omega }^2 $分别代表加速度变化和角速度变化。动态阈值$ \tau $通过加权和的形式表达：

$ \tau = \alpha \cdot {\rm{trace}}({n_\omega } + {n_a}) + \beta \cdot (\sigma _{\Delta a}^2 + \sigma _{\Delta \omega }^2)。$

(8)

式中：$ \alpha $和$ \beta $为权重参数，可以通过实验数据来调整。在实际的预积分过程中，每次积分的时间间隔$ \Delta t $不应超过计算出的阈值$ \tau $。

这些优化方法结合了滑动窗口技术和自适应时间间隔策略，旨在提高在水面高动态变化场景下的IMU预积分精度，同时减少计算负荷。

1.4 水波干扰抑制

水波干扰是USV激光SLAM中的一个主要问题。水面上的杂波是由于海浪、风、潮汐等因素引起的激光反射，它会导致激光雷达数据失真，影响地图构建的准确性。为了解决这个问题，本文提出一种基于复合滤波的水波干扰抑制方法。

设激光雷达数据点集为D = {p_i = (x_i, y_i, z_i)}，其中p_i为三维空间中的点，算法流程图如图4所示。

1）滤波去噪。在分析USV自主靠离泊环境中水波对激光雷达数据的影响时，注意到水波引起的激光反射集中在无人艇周边区域。本文采用了一种结合直通滤波器和条件滤波器的复合滤波策略，以实现对水波噪声的有效剔除：首先，应用直通滤波器去除与无人艇周边区域不相关的点云数据，数学表达为：

$ \begin{split}{c}D_{\text{pass}}=& \left\{\boldsymbol{p}_{\boldsymbol{i}} \in \boldsymbol{D}^{\prime} \mid x_{i} \in\left[x_{\min }, x_{\max }\right], y_{i} \in\left[y_{\min }, y_{\max }\right],\right. \\ &z_{i} \in\left[z_{\min }, z_{m a}\right]\}。\end{split}$

(9)

式中：D_pass为经过直通滤波后的点云，x_min、x_max、y_min、y_max、z_min、z_max为预设的维度范围；接着，应用条件滤波器进一步去除不符合特定条件的点云数据，数学表达为：

$ {D_{cond}} = \left\{ {p_i} \in {D_{pass}}|{I_i} < {k_1} + \frac{{{k_2}}}{{{d_i}}}\right\}。$

(10)

式中：D_cond为经过条件滤波后的点云；I_i为激光点云的反射强度；k₁和k₂为根据实验数据调整得到的参数；$ {d_i} = \sqrt {{x_i}^2 + {y_i}^2 + {z_i}^2} $；最终经过复合滤波处理的点云输出为D'，数学表达为：

$ {D'} = {D}_{{pass}} \cap {D} _{ {cond} } 。$

(11)

这种策略显著提高了激光雷达数据处理的精确度和效率，从而优化了点云数据的整体质量。

2）特征提取。对激光雷达数据进行边缘特征检测，以识别出环境中的边缘特征点^[9]，过程如下：在点云数据集D'中选取时刻t的一个点p_i，在p_i点的同一竖直线上，分别向左右各查找5个点，形成一个集合S，接着计算集合S中每个点的平滑度：

$ c = \frac{1}{{|S| \cdot ||{r_i}||}}||\sum\nolimits_{j \in S,j \ne i} {({r_j} - {r_i})||} 。$

(12)

通过此计算，可得到集合S中每个点的平滑度值。根据这些平滑度值对集合S中的点进行排序，在此基础上，设定平滑度阈值c_t，将平滑度大于c_t的点划分为边缘特征点，而平滑度小于或等于c_t的点划分为平面特征点。

3）水波高度估计。由于水波高度相对较小，可以通过分析边缘特征点与水面的几何关系来估计高度，具体方法是监测垂直方向上边缘特征点的高程变化。假设边缘特征点e_j在没有水波干扰时应在平面z=0上（水面），实际测量的边缘特征点e_j的z坐标为z_j，估计水波高度h_j为：

$ \mathit{h} _{ \mathit{j} } \mathrm= \mathit{z} _{ \mathit{j} } \mathrm- \mathit{z} _{ \mathit{ref} } 。$

(13)

式中：z_ref为参考高度，通常取为水面的估计值。

4）点云数据校正。对于每个激光点云，根据其与水面的距离和水波高度，进行相应的位置调整，以抑制水波干扰。对于激光雷达数据集中的每个点p_i，计算其到估计水面的垂直距离d_i，如果d_i小于或等于估计的水波高度h_j，则认为该点p_i受到水波干扰，对其进行位置调整，校正其z坐标：校正后的点$ {p_i}' $的z坐标为$ {z_i}' = {z_i} - {h_j} $，校正后的点集为$ D'' = \{ {p_i}' = ({x_i},{y_i},{z_i}')\} $，包含了最终经过水波干扰抑制处理的点云数据。

1.5 改进的图优化方法

SLAM系统优化中常遇到局部极值问题，尤其在使用如LIO-SAM的因子图优化框架迭代算法时，这些算法通过最小化误差代价函数来估计轨迹和地图。然而，现实世界的复杂性导致代价函数有多个局部极值，优化算法可能陷入局部最优而非全局最优，影响轨迹估计和地图构建的准确性，降低SLAM性能。特别是在分配给激光SLAM计算资源有限的无人艇平台上，问题更为突出。

为了解决这个问题，本文设计了一种改进的图优化方法，算法流程图如图5所示。

1）改进了一种基于关键帧的优化策略。关键帧选取是为了尽可能减少计算量的同时，保留足够的信息以确保地图的准确性和完整性。关键帧的选择基于特定的准则，如距离阈值或角度阈值。

本文采用时间间隔阈值$ \Delta t $、空间位移阈值$ \Delta d $和角度旋转阈值$ \Delta \theta $作为判断参数。对于每个新到达的激光雷达帧，在时间t上，计算其与上一个关键帧k之间的相对位移$ (\overrightarrow d t,k) $和旋转$ (\Delta {R_t},k) $。关键帧选取的判断依据为满足以下3个条件之一：

$ \left\{ \begin{gathered} t - {t_k} \geqslant \Delta t ，\\ ||{\overrightarrow d _t},k|| \geqslant \Delta d，\\ \angle (\Delta {R_t},k) \geqslant \Delta \theta 。\\ \end{gathered} \right. $

(14)

式中：$ || \cdot || $为欧氏距离；$ \angle ( \cdot ) $为旋转角度。若满足上述任一条件，将当前帧设置为新的关键帧，并更新关键帧集合。考虑到USV的不稳定性和水面环境特征退化的影响，对这些阈值调整为{$ \Delta t $=1s，$ \Delta d $=1m，$ \Delta \theta $=5°}，这种调整旨在提高SLAM系统在自主靠离泊环境中的鲁棒性和准确性。

2）引入一种自适应权重调整机制。在优化过程中动态调整观测约束的权重，通过实时评估观测约束的质量，对与当前状态估计一致的观测赋予较低权重，对与当前估计差异大的观测赋予较高权重。

先初始化权重：$ {w}_{i}={w}_{init}，\forall $i，其中$ {w}_{init} $是初始权重值，定义状态估计问题为最小化加权代价函数的过程，其中代价函数量化了估计的地图和轨迹与实际观测数据之间的一致性。状态估计$ \mathop x\limits^ \wedge $可通过式(15)这个优化问题获得：

$ \mathop x\limits^ \wedge = \arg \mathop {\min }\limits_x \sum\limits_i {{w_i}} \cdot {f_i}(x,{z_i})。$

(15)

式中：x为状态变量；z_i为第i个观测数据；w_i为观测数据的权重；$ {f_i}(x,{z_i}) $代表与第i个观测数据相关的代价函数。

接着进行观测约束评估：每个观测数据与当前状态估计之间的一致性通过计算残差r_i来评估，如下式：

$ {r_i} = {z_i} - {h_i}(\mathop x\limits^ \wedge )。$

(16)

式中：$ {h_i}(\mathop x\limits^ \wedge ) $为根据当前状态估计$ \mathop x\limits^ \wedge $计算出的预期观测值。

此时进行权重调整：根据残差的大小自适应调整权重，以强化与当前状态估计差异较大的观测数据影响。权重调整策略定义如下式：

$ {w_i} = \{ \lambda \cdot {w_i},{\text{if}}\parallel {{\text{r}}_i}\parallel < \delta \frac{1}{\lambda } \cdot {w_i},{\text{if}}\parallel ri\parallel \geqslant \delta 。$

(17)

式中：$ \lambda > 1 $为权重调整因子，$ \delta $为残差阈值，用于区分一致性好的观测与一致性差的观测。当残差小于$ \delta $时，即观测数据与状态估计高度一致，降低该观测数据的权重；当残差大于或等于$ \delta $时，即观测数据与状态估计存在显著差异，增加该观测数据的权重。

这种策略旨在陷入局部极值时，通过强化关键观测约束帮助算法逃逸并探索新解空间。此外，本研究还整合闭环检测机制，在检测到闭环事件时调整权重，利用环境结构信息帮助系统从局部极值中恢复。

闭环检测通过比较当前观测数据与历史数据，识别环境中已知结构的重现，本文采用了文献[10]中基于正态分布变换（Normal Distributions Transform，NDT）粗配准与ICP精配准结合的匹配算法及两段式闭环检测算法进行闭环检测，当检测到闭环事件C时，相应地调整观测约束的权重。计算方式为：

$ {w_i} = {w_i} \cdot \beta ,\forall i \in C。$

(18)

式中：$ \beta > 1 $为闭环检测后的权重增强因子，至此完成自适应的权重调整。

这一综合策略的实施旨在提升无人艇在进行激光SLAM时的定位精度和地图构建质量，特别是在动态和复杂环境条件下的表现。

2 实验内容

为了严谨地评估所提出的改进LIO-SAM算法的性能表现，本研究在自主靠离泊环境中进行了实地试验。在试验过程中，选用了RS-LiDAR-32型激光雷达、XSENS MTi-300型惯性测量单元（IMU）以及全球定位系统（GPS）作为数据采集设备；算法的实现采用了C++编程语言，并在搭载Intel i7处理器的笔记本电脑上运行，操作系统为Ubuntu 20.04 LTS Linux，同时利用机器人操作系统（ROS）进行数据通讯和处理，该系统平台如图6所示。

为了验证算法的有效性，在山东日照、江苏连云港港口和江苏连云港徐圩新区3个不同地方收集了相应的数据集，分别命名为水域A、水域B和水域C。通过对实验数据进行分析，得出以下结论：

1）所提出的改进LIO-SAM算法在定位精度方面优于原始LIO-SAM算法，特别是在高速运动下，改进算法的定位误差明显减小。图7所示为SLAM技术所构建的三维场景地图与Google Earth的卫星图像的对比。

对实验中构建的水域三维场景地图进行关键特征的细节放大分析，以评估其分辨率。部分放大细节效果见图8。通过对这些细节图和特征点间的数据测量计算，证实了3个水域的三维场景地图分辨率均达到了或超过0.1 m，满足USV对三维场景地图精度的需求。

2）水波干扰抑制方法有效地降低了水波对激光SLAM的影响。实验结果表明，改进算法在存在水波干扰的情况下，地图构建的准确性明显提高，部分水域的水波干扰抑制前后效果对比如图9所示。

假设船舶周围r米区域内的原始点云个数为N，经过算法滤波后，点云个数为N'。剔除率可以表示为$ {P}{t}=({N-N{'}})/{N} $，定量评估显示，所提出的水波干扰抑制算法在3个水域环境中的剔除率均超过90%，显示出较高的抑制能力和稳定性（见表1）。

3）本文提出的改进图优化方法有效地克服了无人艇平台上由于激光SLAM部分的计算资源限制而面临的局部极值问题。实验证明，改进后的算法在收敛速度和稳定性方面均优于原始的LIO-SAM算法。此外，改进算法使得SLAM场景地图的刷新率达到了每秒一次，支持13倍速度回放，提高了地图构建的实时性，满足了USV进行自主靠离泊操作时对三维场景地图的实时性需求。

3 结　语

本文针对自主靠离泊环境中USV激光SLAM算法的挑战，提出了一种改良的LIO-SAM算法。该算法引入了一种基于复合滤波的水波干扰抑制策略，改进了IMU预积分处理和后端因子图优化过程，构建了一个基于紧耦合的算法模型，以提升性能。通过3个水域数据集进行实验验证，所提出的算法在提高定位精度、抑制水波干扰和解决局部极值问题上表现出色。未来，计划进一步优化算法，考虑融入语义特征，以增强其在复杂环境中的鲁棒性。