USV-Tracker：一种高效的水面调查的USV跟随系统
USV-Tracker: A Novel USV Tracking System for Surface Investigation with Limited Resources
论文链接：https://www.sciencedirect.com/science/article/abs/pii/S0029801824025344?dgcid=coauthor
本文提出了一种专为水面调查和目标跟随等实际应用而设计的新型无人水面航行器 (USV) 跟随系统，USV-Tracker。该系统解决了三个主要问题：感知稳健性、跟随遮蔽性和规划效率。这项工作的贡献是多方面的：
（1）利用扩展卡尔曼滤波器 (EKF) 的多传感器融合框架来增强目标检测和定位精度，集成摄像头、激光雷达、GPS 和 IMU 传感器的数据。
（2）分层运动规划算法，可生成遮挡避免轨迹并采用虚拟弹力约束来保持适当的相对跟随位置关系。在密集障碍物环境中，该算法倾向于更接近目标并结合 FOV 方向约束以确保稳定的感知。
（3）一种可见性感知控制策略，通过基于 EKF 的轨迹预测确保目标的持续可观测性。在Gazebo 中的仿真测试和更加具体实物实验验证了系统的有效性和稳健性，证明了其在现实场景中的适用性，并且该系统的运行完全由一台受限的机载计算机支撑，凸显了该系统的实用性。该系统可用于非结构化水域场景中的目标监视跟随，也可运用于多USV的自适应编队或多目标跟踪。
一、引言
近年来，无人机和无人车在各个行业的广泛应用推动了无人驾驶技术的快速发展。USV跟随系统用于环境监测和表面调查等应用，旨在使USV能够稳定地跟随目标。然而，大多数现有的研究仍然停留在理论层面，并没有在实践中得到广泛采用。以前的工作主要集中在先进的控制理论上，努力实现与目标的位置一致性。现实世界的应用涉及更大的复杂性，包括遮挡和与障碍物的碰撞、暂时的目标丢失以及对计算实时性的需求。
对于实用的USV跟随系统，必须解决三个关键挑战：感知鲁棒性、目标跟随遮蔽性以及感知和规划之间的紧密耦合。感知鲁棒性要求通过利用多个传感器的优势，在不同的环境中保持一致的性能。单目相机和多光束激光雷达的融合增强了感知鲁棒性，相机提供了精确的小物体识别，激光雷达提供了可靠的距离估计和局部映射。此外，EKF和轨迹预测算法进一步提高了目标位置检测的准确性，并预测了目标未来的位置。目标跟随遮蔽涉及将预测的目标轨迹整合到规划算法中。高效的弹性规划算法生成灵活且避免遮挡的轨迹，同时保持最佳跟随距离并确保目标保持在视场（FOV）内。弹性规划方法将船体的三个自由度纳入运动学规划，虚拟力机制确保USV相对于目标保持适当的位置和方向。这些算法针对紧凑型车载计算机上的实时操作进行了优化，确保了连续稳定的跟随。感知和规划框架之间的紧密耦合至关重要。感知提供的准确目标轨迹信息对于有效规划至关重要。规划过程考虑了成功感知的必要条件，例如将目标保持在相机的视场内。这种集成增强了系统的鲁棒性，即使目标暂时丢失，USV也能使用EKF预测的轨迹来引导USV继续跟随。
本文提出了一种名为USV Tracker的新型跟随系统，将这些组件集成到一个系统中。该系统在仿真场景和带有常见传感器的紧凑型USV原型机上进行了验证。平均目标定位误差为亚米，明显小于目标的尺寸。USV系统在仿真和物理环境中长时间稳定地跟随目标，突显了其实用性。
二、具体方法介绍
A. 感知模块和目标轨迹预测
USV跟随系统中的感知模块通过集成来自多个传感器(包括摄像头、激光雷达、GPS和IMU)的数据，实现精确的实时3D定位。创新之处在于EKF的双重应用: 一是稳定三维坐标，二是预测目标的运动轨迹。在感知模块中结合使用EKF进行稳定和轨迹预测，为动态环境下的实时目标跟随提供了一个鲁棒和高效的解决方案。
B. 基于可视性感知的跟随轨迹规划
欠驱动的USV不能在水中横向推进，而FOV的方向与USV的航向直接关联，这意味着在路径规划阶段必须仔细考虑跟随者对目标的可见性。因此，采用多目标hybrid A*算法作为前端路径搜索方法，获得无遮挡、无目标丢失的初始跟随路径。
USV系统通常是欠驱动的，并且受到非完整约束，这使得运动规划非常复杂。我们先假设USV是一个完全驱动的系统，并将其位置和朝向作为平坦输出，简化了USV系统状态和输入的平坦变换。所以USV系统状态可以参数化为：
USV的输入的平坦变换表达式为：
我们采用MINCO来描述轨迹，使跟随轨迹能在构建的优化问题上，从时间和空间两个维度上灵活调整。基于可见性感知的跟随轨迹优化问题中的非线性约束转化为惩罚项，这里采用的罚函数都满足2阶可导且导函数连续。因此，一般USV跟踪的无约束优化问题定义如下：
其中主要包含的约束项有可视性跟随约束、弹性距离约束、欠驱动运动约束、动态可行性约束、环境避障约束、轨迹平滑约束以及时间调整约束。最后，采用L-BFGS解决该无约束的数值优化问题。
三、实现效果与应用测试
A. 目标检测效果测试结果
在目标旋转过程中，由于姿态的变化会产生局部误差。此外，随着目标速度的变化，击中目标的激光雷达光束数随距离的变化而动态变化，导致点云聚类后质心发生偏移。这些因素造成短期波动，平均误差为16cm，明显小于目标物体的尺寸，在可接受的误差范围内。进而利用EKF进行轨迹预测的后处理为后续的轨迹规划提供了有价值的信息。
B. USV跟随轨迹的运动可行性试验
目标船只在开阔水域沿激进路径航行，我们设定测试的期望跟随距离为7米。由下图可知，baseline方法和elastic tracker的目标跟随结果是扭曲的，并且在转弯时出现目标丢失的现象。相比之下，我们的方法联合优化了位置和航向，使目标保持在FOV的适度区域内。此外，我们的方法引入了USV运动约束，使得仅使用PID控制器就能以较小的轨迹跟踪误差，轻松跟踪规划的轨迹。
C. USV-Tracker在实物平台上的测试
在实验中，目标USV沿着一条复杂的路径前进，而一艘4米长的皮划艇作为障碍。在没有通信和动态目标干扰的情况下，跟随USV保持了稳定的性能，证明了系统在现实条件下的鲁棒性和准确性。
USV-Tracker系统配备了多个传感器和一个机载NUC，可以实时运行所有算法。目标感知模块利用了大约4个CPU核和不到1.5 GB的内存。具体而言，传感器驱动器利用了约8%的CPU和160MB的内存；环境映射占用了3%的CPU和130MB的内存，目标检测占用了12%的CPU和530MB内存，而多传感器融合滤波消耗了2%的CPU和150MB内存。感知算法保持12Hz的检测输出频率，确保在动态条件下的响应性。
轨迹规划模块中初始路径搜索消耗9%的CPU和225 MB内存，轨迹优化占用6%的CPU和110 MB内存，底层控制使用1%的CPU和72 MB内存，FCU驱动所需的CPU和65 MB内存不到1%。轨迹规划的计算时间通常保持在25ms以下，满足实时USV跟随的要求。
四、结论
我们研究了在跟随过程中影响USV跟随效率和能见度的因素，并提出了一种新颖的USV跟随系统。尽管存在传感器限制和环境障碍，但该系统仍能确保感知鲁棒性和跟随遮蔽性。该系统基于多传感器融合感知和可见性感知轨迹规划方法而构建。通过仿真和实物实验验证了所提出的USV跟随系统的效率和鲁棒性。在未来的工作中，我们计划用专业级仪器取代实验设备，并将USV-Tracker扩展到多USV的领域，研究使用多艘USV在海域监测场景中进行自适应编队或跟随多个目标。