自动化装船场景下船体实时跟踪系统设计与应用

Design and Application of Real-Time Ship Hull Tracking System for Unmanned Loading Scenarios

摘要: 在无人装船作业中，高精度船体位姿跟踪是保障溜筒精准对位与作业安全的关键。针对传统PLC数据点云对齐方法精度有限的问题，本文提出一种基于预建船体地图的两阶段实时船体跟踪方法。该方法首先构建全局地图，装船时利用PLC数据为新点云提供初始位姿进行粗定位，再驱动ICP算法与地图进行精配准，实时解算船体精确位姿。实验表明，该方法在整个装船测试流程中可以实现稳定、可靠的跟踪效果，为无人装船系统的智能控制提供了稳定可靠的空间基准。

Abstract: In unmanned ship loading, accurate real-time tracking of the ship’s 6-DOF pose is vital for safe, collision-free, and precise chute positioning. To overcome the limited accuracy of PLC-only kinematic methods, we propose a robust two-stage approach: first, PLC encoder data provides a coarse initial alignment of each new LiDAR scan to a pre-built 3D ship hull map; then, the ICP algorithm refines this alignment to achieve high-precision pose estimation. Field tests at a working port show the method delivers stable and reliable tracking throughout the entire loading cycle while the ship remains docked—and intentionally fails when the ship departs, confirming its responsiveness to real conditions. This provides a dependable, real-time spatial reference essential for intelligent control of unmanned loading systems.

1. 引言

在现代化港口散货装船作业中，实现溜筒对移动船舱的自动精准跟踪是无人化技术落地的核心。溜筒末端与船舱边缘的微小碰撞都可能造成严重的安全事故，因此，对船体位姿进行高频率、高精度的实时感知是系统安全运行的基石[1]。

当前船体位姿感知技术主要包括三类：(1) 基于GNSS/RTK与惯性导航的方法，可提供全局绝对位姿，但受多路径效应和遮挡影响，在港口高架结构下定位精度常劣于0.5 m [2]；基于视觉的方法(如单目/双目SLAM)成本低、纹理丰富，但在低光照、无纹理或强反光的船体表面易失效，且难以恢复尺度；基于激光雷达的方案具有高精度、抗光照干扰等优势，近年来被广泛应用于船舶检测与跟踪。然而，多数现有工作依赖实时点云分割，计算开销大，且在背景复杂(如码头、料堆)场景下鲁棒性不足[3] [4]。相比之下，本文利用预建高精度船体地图作为先验，结合PLC运动学初值与ICP精配准，在保证精度的同时显著提升跟踪稳定性与计算效率，特别适用于结构已知、重复作业的装船场景。该方法通过“PLC粗定位 + ICP精配准”的双阶段框架，旨在提升船体跟踪的稳定性，为无人装船系统的智能决策提供可靠的空间基准。

2. 船体跟踪系统设计

本系统的核心目标是在动态装船作业过程中，实现对船体六自由度位姿(位置与姿态)的高频率、高精度、高鲁棒性实时估计。为达成此目标，系统采用“预建地图 + 实时配准”的范式，其核心创新在于构建了一个融合运动学先验与几何优化的双阶段跟踪框架。本章将重点阐述该框架的实时跟踪阶段，其理论基础与工程实现细节如下。

2.1. 系统总体架构与数学模型

设预建的全局船体点云地图为一个静态点集$ℳ={\left\{m_i\in R^3\right\}}_{i=1}^{N_m}$ ，其定义于世界坐标系$\mathcal{W}$ 中。在任意时刻$t$ ，系统采集到一帧新的观测点云${\mathcal{S}}_t={\left\{s_j\in R^3\right\}}_{j=1}^{N_s}$ ，该点云初始定义于雷达局部坐标系$ℒ$ 中。跟踪任务

的本质是求解一个刚性变换矩阵${\text{T}}_{\text{t}}^{\text{W}\leftarrow \text{L}}\in SE\left(3\right)$ ，该矩阵能将观测点云${\mathcal{S}}_t$ 从局部坐标系$ℒ$ 变换至世界坐标系$\mathcal{W}$ ，并使其于地图$ℳ$ 在几何上达到最优对齐。数学上，该问题可表述为一个非线性最小二乘优化问题：

$T_t^{W\leftarrow L}=\arg \underset{T\in SE\left(3\right)}{\min }{\displaystyle \sum }_{s_j\in {\mathcal{S}}_t}\underset{m_i\in ℳ}{\min }{\Vert \text{T}\cdot s_j-m_i\Vert }^2$

直接求解此优化问题可能是不准确的。为此，本系统提出一种分阶段的求解策略：首先利用运动学模型提供一个粗糙的初始解，再通过ICP迭代优化算法对位姿进行细化。

2.2. PLC粗定位

在实时跟踪阶段，系统的核心任务是将每一帧新采集的观测点云${\mathcal{S}}_t$ 从其原始的雷达局部坐标系$ℒ$ 精确转换至全局地图所定义的世界坐标系$\mathcal{W}$ 。由于雷达固定安装于装船机溜筒座上，其空间位姿随大臂的回转、俯仰及大车平移而动态变化，因此，直接使用原始点云进行配准将导致严重的空间错位。

为解决此问题，本系统采用基于装船机运动学模型的实时坐标变换机制，实现观测点云的初步对齐。该机制的核心是利用PLC系统同步输出的高精度编码器数据，结合装船机的已知机械结构参数，通过正向运动学链，实时解算雷达坐标系相对于世界坐标系的刚性变换矩阵。本方案中的装船机采用典型的门座式三自由度结构，其运动链可简化为：大车沿码头轨道方向平移(定义为世界坐标系的X轴)，带动上部回转平台整体移动；回转平台绕垂直轴(Z 轴)旋转，实现水平方向的角度调整；其上安装的俯仰臂可绕横向水平轴(Y 轴)摆动，用于调节溜筒高度。激光雷达固定安装于俯仰臂末端(即溜筒座附近)，其空间位姿由上述三个关节变量共同决定。通过装船机 PLC 提供的实时编码器数据，可依据该运动链顺序构建从世界坐标系到雷达坐标系的齐次变换矩阵，从而实现对每帧点云的粗定位。

具体而言，设在时刻$t$ ，PLC提供的运动参数包括：

a) 大车沿轨道方向的平移位移$d_x\left(t\right)$ ；

b) 回转机构绕垂直轴的旋转角度${\theta }_r\left(t\right)$ ；

c) 俯仰机构绕水平轴的旋转角度${\theta }_p\left(t\right)$ 。

根据装船机的多刚体运动链结构，上述参数可依次构造三个基本齐次变换矩阵：

a) 平移变换矩阵$T_{\text{trans}}\left(t\right)$ ：描述大车平移引起的坐标系整体移动；

b) 旋转变换矩阵$T_{\text{rot}}\left(t\right)$ ：描述回转机构引起的绕垂直轴的旋转；

c) 俯仰变换矩阵$T_{\text{tilt}}\left(t\right)$ ：描述俯仰机构引起的绕水平轴的旋转。

将这三个变换矩阵按运动链顺序相乘，即可得到从世界坐标系$\mathcal{W}$ 到雷达坐标系$ℒ$ 的完整变换矩阵：

$T_{\text{init}}\left(t\right)=T_{\text{trans}}\left(t\right)\cdot T_{\text{rot}}\left(t\right)\cdot T_{\text{tilt}}\left(t\right)$

该变换矩阵$T_{\text{init}}\left(t\right)\in SE\left(3\right)$ 包含了完整的旋转和平移分量，可将当前帧点云${\mathcal{S}}_t$ 转换至世界坐标系$\mathcal{W}$ 下：

$s_t^{world}=T_{\text{init}}^{-1}\left(t\right)\cdot s_t$

由此，获得初步对齐后的观测点云$s_t^{\text{world}}$ 。该点云虽仍存在由传感器噪声、机械形变或船体自身微动引入的残余误差，也存在船体运动引入的对齐误差，但其整体几何结构已与预建地图M处于同一空间基准下，且船体位姿偏差与转换前已大幅降低。这为后续的精配准阶段提供了高质量的初始解。本阶段的粗配准过程具有绝对坐标系约束特性：每一帧的初始位姿均独立计算，不依赖于前一帧的跟踪结果，从而从根本上避免了位姿估计误差的累积传播，保障了系统在长时间运行中的鲁棒性与可靠性。

2.3. ICP精配准

在获得初始位姿后，系统调用ICP算法对初始变换进行精化。算法的输入为：

a) 源点云：初步对齐后的观测点云$s_t^{world}$ ；

b) 目标点云：全局地图$ℳ$ ；

c) 初始位姿：单位矩阵$\text{I}$ (因为$s_t^{world}$ 已在世界坐标系中。

算法流程为

a) 体素下采样：对源点云$s_t^{world}$ 进行体素网格滤波，保留每个体素内的一个代表性点，以降低计算复杂度并抑制噪声；

b) 最近邻搜索：对于下采样后的每一个三维点，在目标地图$ℳ$ 中搜索其最近邻点$m_{nn}$ ；

c) 对应点筛选：为提升鲁棒性，仅保留距离小于阈值的点对作为有效对应；

d) 位姿估计：基于筛选后的点对集合，使用奇异值分解求解最优的刚性变换$\text{Δ}T$ ，以最小化点对间的欧氏距离平方和；

e) 位姿更新：将当前估计的位姿更新为$T_{current}=\text{Δ}T\cdot T_{last}$ ，$T_{last}$ 是上一次迭代计算出的位姿，$T_{current}$ 是当前时刻计算得到的位姿。

f) 收敛判断：若变换增量$\Vert \text{Δ}T-\text{I}\Vert$ 小于预设阈值或达到最大迭代次数，则算法终止；否则返回步骤2。

最终经过ICP迭代优化后，得到一个精细化后的变换矩阵$T_{ICP}\left(t\right)$ 。由于输入的源点云经过了雷达坐标系至世界坐标系的转换，最终当前帧的船体点云到地图点云的位姿变换表示为：

$s_t^{final}=T_{ICP}\left(\text{t}\right)T_{\text{init}}^{-1}\left(t\right)s_t$

该变换直接描述了当前船体$s_t$ 相对于预建地图的精确位姿，$s_t^{final}$ 表示对齐至预建地图后的点云。

本系统采用点到点ICP，关键参数设置如下：体素下采样尺寸为0.5 m，以平衡精度与实时性；最近邻距离阈值设为0.3 m，用于剔除异常对应点；最大迭代次数为50，收敛阈值为平移增量 < 1 mm且旋转增量 < 0.1˚。上述参数通过在离线数据集上进行网格搜索确定，在保证配准精度的同时，单帧处理时间控制在20 ms以内，满足系统10 Hz的实时性要求。上述设计为装船机装船动态场景下的船体点云跟踪提供了一种通用的、理论完备的解决方案。通过此方法，系统能够实时跟踪当前帧中的船体点云，为装船运动控制等高级应用提供了可靠的数据基础。

2.4. 三维船体地图的构建

预建船体地图是本系统实现高精度跟踪的基础。地图构建阶段在船舶空载、静止停靠于码头时进行。采用安装于装船机溜筒座上的激光雷达对船体进行多角度环绕扫描，共采集约200帧点云数据。每帧点云首先通过PLC提供的装船机位姿进行粗对齐，随后采用全局优化的配准算法进行闭环优化，消除累积漂移。最终，所有点云经统一坐标变换后融合为一张全局一致的静态船体点云地图。为提升地图质量，对融合后的点云进行体素滤波，并手动剔除码头、缆绳等非船体结构。最终地图覆盖船体甲板、舱口围板及主要上层结构，空间分辨率达5 cm。该地图作为后续实时跟踪的静态目标模型，具有高几何保真度与结构完整性。

3. 应用测试与效果分析

为全面、客观地评估本文所提出的“PLC粗定位 + ICP精配准”双阶段框架在真实工业场景下的性能，本研究在长江中游某大型散货港口进行了实地部署与系统性测试。测试环境涵盖了装船作业的全过程，包括船舶稳定停靠以及最终离港等典型工况，旨在充分验证系统在复杂动态环境下的精度、稳定性与鲁棒性。

3.1. 测试方案设计

测试的核心目标是验证提出的上述两阶段配准方案在实时跟踪场景下的有效性。在装船过程中，系统持续采集新的点云帧，并同步记录PLC编码器数据。本次测试共采集并处理了869帧连续数据，其中第800帧左右对应船舶开始离港的关键时刻。

由于实时跟踪场景中，待配准的源点云包含码头、装船机、料堆等大量非船体背景点，而目标地图$ℳ$ 仅包含纯净的船体结构，传统的基于点云重叠率的评价方法不再适用。为此，本研究设计了一套基于“地图到源点云距离统计”的通用评价体系。其核心思想是直接计算预建地图$ℳ$ 中的每个点到当前帧点云$s_t^{final}$ 中最近邻匹配点的欧氏距离，并对此距离集合进行多维度统计分析，再辅以ICP匹配点的数量作为参考。该方法不依赖于点云分割，直接反映地图与观测数据在几何空间上的对齐质量。

3.2. 测试结果与分析

图1展示了系统在869帧完整测试周期内，实时解算出的船体位姿随时间的变化曲线。可以看出，在0至800帧的船舶稳定停靠阶段，系统输出的船体位置轨迹平滑且波动极小，表明跟踪系统运行稳定，能够持续提供高精度的位姿估计。而在第800帧附近，随着船舶开始离港，地图点云与当前帧点云失去有效重叠，ICP算法无法找到正确的几何对应关系，导致配准失败，输出的位姿发生剧烈且无物理意义的跳变。这一现象与实际物理过程完全吻合，从侧面验证了系统输出的可靠性。

Figure 1. The hull’s real-time pose trajectory is stable from frames 0~800 (berthed) but shows severe, irregular jumps after frame 800 (departure)

图1. 自系统输出的船体实时位姿轨迹。在0~800帧(船舶停靠期)，轨迹平稳；在800帧后(船舶离港)，轨迹出现剧烈、无规律的跳变

Figure 2. Time-varying curves of the six evaluation metrics

图2. 六项评价指标随时间变化曲线

图2展示了六项核心评价指标在整个测试周期内的变化趋势。结果表明，在货船还未驶离码头时，距离相关的评价指标均处于相对稳定状态，Q1 Distance指标的稳定性最为突出。而在货船驶离码头时，所有距离相关的评价指标均出现了同步且急剧的突变。同时，ICP配准得到的匹配点也出现骤降，上述现象与物理事实基本吻合，证明了评价指标体系的有效性和敏感性，能够准确捕捉到跟踪状态的突变。

3.3. 局限性讨论

尽管本系统在晴朗天气和船舶静态停靠条件下表现良好，但仍存在局限性。首先，激光雷达在雨、雾或高粉尘环境中性能显著下降，点云信噪比降低可能导致ICP配准失败。未来可融合毫米波雷达或视觉辅助以提升环境适应性。其次，当前方法假设船体为刚体，未显式建模装船过程中因货物装载导致的船体形变(如中拱/中垂)。若舱口区域发生形变，可能影响局部配准精度。后续工作拟引入弹性配准或动态地图更新机制，以应对非刚性形变场景。此外，本系统依赖高质量的预建地图，对首次作业的新船型需要进行额外建图流程，限制了其泛化能力。

4. 结束语

本文提出了一种结合PLC数据与ICP算法的船体实时跟踪方法，通过每帧点云都利用PLC进行坐标系初始化，再与预建地图配准，有效控制了跟踪过程中的漂移问题。现场测试表明，该方法在船舶停靠期间能保持稳定跟踪，关键指标如Q1距离维持在0.08米左右，离港时指标变化明显，便于系统识别状态异常。相比仅依赖初始位姿的方案，本方法精度更高、稳定性更好，为装船机溜筒的自动对位提供了一种实用、可靠的技术支持。

基金项目

本论文受国家自然科学基金项目资助，项目名称：“融合透视投影几何与深度学习的多关节体三维位姿求解稳定性分析”(62273318)；本论文受中国电力建设股份有限公司科技项目经费委托，受中国水利水电第八工程局有限公司科技项目资助，项目名称：“中电建关于绿色智慧矿山关键技术应用研究”重大科技专项课题11——“智慧码头关键技术研究”。

NOTES

^*通讯作者。