面向任意位置姿态估计的无人艇虚实交互研究

杨少龙，孙延浩，黄 金，向先波

(华中科技大学 船舶与海洋工程学院，湖北 武汉 430074)

0 引 言

无人艇作为海上移动平台，受风、浪、流等作用会产生六自由度运动，包括3 个方向的平动和3 个方向的转动。对于旋转运动，船体不同位置与旋转中心的距离不等，旋转运动产生的线速度不同，而船体任意点的合速度是平动速度与转动速度的矢量和，故实际海上航行无人艇船体各点的合速度矢量均不相等。无人艇的许多作业场景依赖于观测无人艇自身某一局部位置的位姿信息，例如，无人艇的布放和回收、多无人艇对接、无人艇靠离码头、艇载传感器位姿补偿（如摄像头、测扫声呐、避障声呐、对底声呐、激光雷达等）等。为此，如何获取船体任意位置的实时位姿状态成为无人艇智能化发展中信息感知关键技术的重要挑战之一。

船体任意位置实时位姿状态获取，通常需要若干个惯导传感器数据的融合处理。司帆等[1]为克服机翼挠曲变形对机载导弹对准产生的影响，根据主、子惯导的惯性器件输出，得到子惯导所处位置的高精度实时姿态信息。王林等[2]针对单轴与双轴旋转惯导的信息融合问题，设计联合旋转调制策略，实现了单轴旋转惯导定位误差的预测与补偿，提高了系统的导航定位可靠性。刘镇波等[3]提出一种基于分布式IMU 的相对姿态估计方法，能够在系统无高精度惯导及变形统计特性未知的情况下，获取机载关键位置精确的相对姿态。这些研究着重关注多个物理传感器的数据融合测算传感器所在局部位置的实时位姿，虽然精度较高，但却不能满足任意多个测点的位姿实时观测需求。随着无人艇全船布置设备增多，有限个惯导传感器扩容却显著增加了传感系统的复杂度及投资成本。

当前，数字孪生技术的迅速发展为上述需求提供了新的解决思路。数字孪生是一种集成了多物理量、多尺度、多概率的仿真过程，以数字化方式创建物理实体的虚拟模型，通过虚实交互与数据融合实现物理实体的全生命周期服务[4-5]。这个概念最早被提出是用于描述产品的生产制造和实时虚拟化呈现，随后该理念被推广应用到工业产品、大型装备的实时运行监测[6]。数字孪生通常包括物理空间的实体、信息空间的虚拟孪生体、物理空间和信息空间之间的数据和信息交互接口三部分[7-9]。目前，已有多位学者对数字孪生在船舶领域的应用进行了初步研究。李凯等[10]基于数字孪生提出数字化舰船的总体框架，并探索基于数字孪生的产品设计、辅助决策、健康管理等潜在应用场景。陶飞等[11]提出将数字孪生与船舶工业结合，参照数字孪生五维模型，探讨基于数字孪生的船舶设计、制造、运维、使用等全生命周期一体化管理理念。然而，由于缺乏系统的数字孪生理论支撑和应用准则指导，目前在船舶领域数字孪生的研究多集中于理论探讨，鲜有实际应用报道。

因此，面向无人艇任意位置实时姿态估计需求，本文设计基于数字孪生的无人艇虚实结合三维交互系统，通过刚体运动学建立无人艇物理空间时序数据与信息空间几何信息的跨空间融合方法。基于单套状态感知设备获取的信息，无人艇实现在信息空间实时映射来自物理空间的航行状态，并可实时估算船体任意位置的位姿状态。

1 无人艇虚实结合三维交互系统架构

1.1 系统总体框架

如图1 所示，无人艇虚实结合三维交互系统由实体无人艇、三维可视化交互软件以及物理空间与信息空间的交互接口组成。无人艇是系统在物理空间的实体对象，搭载了姿态传感器、位置传感器等传感设备，能够感知航行信息并实时传递给上位机。三维可视化软件是系统进行无人艇姿态三维可视化显示和姿态信息虚实交互的部分，由无人艇虚拟孪生体、UI 界面和Script 组成。信息交互接口主要由地面站软件（QGroundControl，QGC）组成，QGC 在系统中发挥数据中介作用，一方面向无人艇发送任务信息，并接收无人艇传回的航行信息；另一方面从无人艇传回的航行信息中提取位姿数据，发送给三维可视化交互软件。

图1 无人艇虚实结合三维交互系统总体构架Fig.1 Framework of the unmanned surface vehicles' virtual reality fusion three-dimensional interactive system

1.2 系统信息流传输机制

无人艇虚实结合三维交互系统能够把实体无人艇传感器感知到的位置、姿态等信息实时映射为信息空间中无人艇虚拟孪生体的运动信息。从无人艇传感器实时数据的输入到三维可视化软件中无人艇虚拟孪生体运动的输出，信息流需要跨越多个软硬件平台。如图2 所示，在物理空间、信息空间以及数据交互接口中，均设有数据的传递、转化模块，各模块共同组成了系统的信息流传输机制。

图2 无人艇虚实结合三维交互系统数据传输机制Fig.2 The unmanned surface vehicles' virtual reality fusion threedimensional interactive system structure

无人艇与上位机通过无线数传以MAVLINK 协议方式通信，上位机中安装有二次开发的QGC 软件，在QGC 软件中解析MAVLINK 消息包提取传感器数据，并用UDP 封装姿态数据（横滚角、首向角、俯仰角）和位置数据（经度、纬度、相对海拔、速度）。通过本地UDP 套接字，将封装好的数据发送至特定端口。同时，在Unity3D 数据处理Script 中创建Socket 对象，监听该端口，实时接收位姿数据。数据处理Script 把接收到的UDP 数据解包，并对位姿数据进行预处理后分别赋予位置变量和姿态变量。运动控制Script 和任意点速度计算Script 分别调用这些变量实现无人艇虚拟孪生体的三维动态显示和实时位姿状态估计功能。

2 虚拟无人艇及交互设计

2.1 虚拟无人艇孪生体及虚拟场景建立

精确的无人艇虚拟孪生体模型是三维可视化交互软件实现无人艇数字孪生的基础。本文用SolidWorks建立实体无人艇1∶1 三维模型，再用3Dmax 转换为可供导入Unity3D 的.fbx 文件。为了获得逼真的三维仿真效果，在三维可视化交互软件中构建与真实环境相似的虚拟场景，例如基于Terrain 组件建立山脉，利用法线贴图、菲涅尔效应、漫反射着色器以及折射着色器等模拟水体，采用Skybox 及光源模拟天空和光线。

如图3 所示，虚拟场景导入无人艇虚拟孪生体，把附体坐标系的坐标原点设置在无人艇质心位置，xb轴正方向指向船首，zb轴正方向指向左舷，yb轴正方向竖直向上。无人艇虚拟孪生体初始位置位于虚拟场景中央，满足航行试验需求。

图3 虚拟场景及场景中的无人艇虚拟孪生体Fig.3 Virtual scene and twin USV inside

2.2 实测数据驱动三维可视化交互设计

三维可视化交互软件使用QGC 传回的位姿数据来驱动无人艇虚拟孪生体。对于位置数据，将GPS 测量得到的经纬度信息经坐标变换转化到虚拟场景的世界坐标系；对于姿态数据，将欧拉角形式姿态信息转换为四元数，避免万向节死锁问题。

无人艇位姿数据采样率取决于无人艇的控制周期和传感器自身固有属性，本文中位置和姿态数据采样率分别为5 Hz 和50 Hz。而虚拟场景中流畅的画面通常要求帧率在30FPS 以上，为此，对坐标系转换后的位置数据进行线性插值和球面插值补帧，使得无人艇在虚拟场景中的动态画面流畅且过渡平滑。

真实位姿数据驱动无人艇虚拟孪生体动态画面刷新原理如图4 所示。在运动控制Script 中，在每一帧画面刷新时调用一次Update 函数，经差值优化的位姿变量赋值给无人艇虚拟孪生体Transform 属性，更新无人艇虚拟孪生体在下一帧动画中的位姿状态，结合Unity3D 图形引擎进行动态渲染，产生动态画面。

图4 位姿数据驱动无人艇虚拟孪生体原理Fig.4 The principle of position and attitude data driving twin USV

3 实体无人艇及监控设计

3.1 实体无人艇基本组成

本系统实体无人艇为基于Pixhawk 的小型喷水推进无人艇（总长0.66 m，型宽0.20 m），由控制系统、动力系统、感知系统、通信系统等构成，各系统部件沿中纵剖面布置在无人艇两舷。感知系统主要由GPS 和惯导组成。

3.2 实体无人艇监控系统

实体无人艇监控系统由无人艇和上位机组成，如图5 所示。上位机运行QGC 软件，通过3DR 433 MHz数传与无人艇通信，实现向无人艇发送指令，同时接收无人艇传回各种状态。本系统上位机可规划无人艇的航行任务、航路、下达指令；接收无人艇的实时状态反馈，在QGC 和三维可视化交互软件界面实时显示无人艇的航行状态、地图轨迹等信息。

图5 无人艇监控硬件系统Fig.5 USV monitoring hardware system

4 船体任意点速度估计原理与验证

4.1 虚实结合跨空间位姿数据融合原理

根据实体无人艇的位姿时序数据，结合虚拟孪生体几何模型数据，本文实现了虚实结合跨空间数据融合，可估算船体任意位置的实时速度。

将无人艇视为刚体，虚拟孪生体质心是旋转中心，也是附体坐标系原点Ob（与实体无人艇质心位置相同）。如图6 所示，船体的自身旋转运动可表示为ω=[ωxωyωz]T，任意点i的速度为平移速度与转动线速度 ω ×r的向量和，即Vi=VOb+ω×r，其中 r=[rx ry rz]T是i点对Ob的向径。

图6 任意点i 的速度分析Fig.6 Velocity analysis of arbitrary point i

2）无人艇平移运动是前进、横漂、起伏3 个方向的叠加，即 VOb=[VObx VOby VObz]T。平移速度VOb由GPS 与惯导数据采集至Pixhawk 由扩展卡尔曼滤波计算得出[12]。无人艇在水面起伏运动通常可忽略，即假设VOby=0 。VObx与VObz的 合速度用VG表示，其标量值为 vG。由于横漂的存在，导致无人艇平移运动方向（VG方向）与首向（VObx方向）存在偏差，其夹角用偏航角β 表示。

图7 首向角与航向角分析Fig.7 Analysis of heading angle and course angle

根据公式Vi=VOb+ω×r，可以写出任意点i在某一时刻t的离散形式速度表达式：

按照矢量求和方法可计算i点合速度值vi，最终可得vi与无人艇位姿信息的关系为：

其中，xie,yie,zie为无人艇虚拟孪生体在世界坐标系的位置，由GPS 数据经坐标变换获得；任意点的附体坐标可由Unity3D 引擎提供的Ray 组件获取。

4.2 无人艇任意位置实时速度估计湖试验证

进行本文开发的无人艇虚实结合三维交互系统及任意位置速度估计功能的湖试。以某一段直角转弯航路为例分析，航行轨迹如图8 所示。

图8 试验航行轨迹Fig.8 Experimental navigation trajectory

如图9 所示，三维可视化交互软件速度估计界面，鼠标点击无人艇虚拟孪生体任意位置可计算该点实时速度。在无人艇船体上任取四点作为速度估计观测点，取Obxz平面内一点A（0.1，0，0.1）、Obyz平面内一点B（0，0.05，0.46）、附体坐标系原点上方的投影点C（0，0.08，0）和甲板上任一点D（-0.2，0.05，-0.1）。

图9 无人艇速度估计界面及试验点位置分布Fig.9 Speed estimate UI for USV and experimental point layout

受限于本试验无人艇尺寸，4 个试验点与无人艇质心Ob距离较短。试验点的实际速度与质心位置速度由于角速度变化引起的偏差不明显。为说明常规无人艇[13]（船长尺度5m 及以上）的任意点的速度差异，将所选试验点的坐标值放大10 倍，即A′（1，0，1）、B′（0，0 .5，4.6）、C′（0，0.8，0）、D′（-2，0.5，-1）。对实际湖试航行过程各个试验点进行速度对比分析，结果如图10～图13 所示（E和D分别表示试验点相对Ob点速度偏差的期望和方差）。

无人艇在415～435 s 和435～455 s 两个时间段内直线航行，为降低转弯过程轨迹跟踪误差，无人艇自主程序控制会在435～440 s 时间段降速完成直角转弯过程。由图10～图13 可见，试验点A-D的实际速度均与无人艇质心Ob不等，但由于各点相对质心距离较短，因此速度偏差并不显著。然而，对于常规尺度无人艇，当坐标值放大10 倍后，试验点A′-D′与无人艇质心Ob的速度偏差明显增大。以D点和D′点为例，ED为2.62×10-4m/s，而ED′达到7.74×10-2m/s。尽管坐标值仅变化10 倍，但2 个试验点的平均速度偏差却相差2.95×102倍。此外，值得注意的是，在直角转弯过程，无人艇会产生绕yb轴的明显转艏运动，旋转角速度会显著大于直航段，因此各个试验点在435～440 s时间段内出现了较为显著的速度偏差。尤其当坐标值放大10 倍后，无人艇的船体任意点速度会与质心Ob位置的明显不同。由此，对大尺度无人艇，由于自身姿态变化引起的船体局部位置的速度偏差将更加显著，这对无人艇局部位置布置的设备运转将产生重要影响。

图10 航行过程A 点、A′点与 Ob点的速度对比Fig.10 Velocity comparison between point A,A′ and point Ob

图11 航行过程B 点、B′点与 Ob 点的速度对比Fig.11 Velocity comparison between point B,B′ and point Ob

图12 航行过程C 点、C′点与 Ob 点的速度对比Fig.12 Velocity comparison between point C,C′ and point Ob

图13 航行过程D 点、D′与 Ob点的速度对比Fig.13 Velocity comparison between point D,D′ and point Ob

5 结 语

本文建立基于数字孪生的无人艇虚实结合三维交互系统，实现了在信息空间真实映射物理空间无人艇航行状态，并可在信息空间估计无人艇任意位置速度。该系统以自研无人艇为物理空间实体对象，使用Unity3D在信息空间搭建了无人艇虚拟孪生体，利用QGC 软件建立物理空间与虚拟空间的交互接口。利用虚实结合跨空间姿态数据融合原理，将物理空间实时观测位姿信息与信息空间无人艇几何模型信息融合。基于有限数量的传感器实现了对无人艇全船任意位置的实时姿态估计。通过湖试验证了本系统的三维交互与速度估计功能。当无人艇角速度增大导致姿态变化较大时，无人艇局部位置的合速度将与质心位置的偏差明显增加，且随着与质心间距增加，这种偏差效应更加显著。

本文建立的无人艇虚实结合三维交互系统有助于船舶智能感知技术发展，但在信息空间估计数据服务于物理空间方面仍需进一步探究.