无人船自主航行目标方位测量技术研究

潘怡南，赵馨，常帅

（1.长春理工大学 电子信息工程学院，长春 130022；2.长春理工大学 空间光电技术研究所，长春 130022）

无人船（USV）是一种具有自主完成目标识别、定位、特性参数感知等功能的智能水面平台，能够完成自主避障、路径规划、自主航行等功能［1-2］。USV在气象监测、海上搜救、海洋勘探、环境监测等海事应用中发挥着越来越重要的作用［3］。USV在自主航行时，为了满足上述实际应用需求，不仅要实时获取目标的相对位置信息（方位角、俯仰角、距离），还要进一步获取目标在大地坐标系中的绝对位置坐标（纬度、经度、大地高）［4］。然后将这些信息在电子海图上进行标定，以便于对目标及周围环境进行感知分析和监测［5］。

无论是在军用还是民用中，无人船需要配备相应的传感器模块，例如雷达、激光测距机、视觉设备、卫星导航系统和光电传感器等设备；实现行进路线上周围目标识别、定位、感知，获得动、静目标位置、速度、航向角等信息，从而合理规划行进路径、周围目标进行监测［6］。檀立刚等人［7］和王晶等人［8］搭建了机载光电成像测量平台，通过机载光电设备对目标指向矢量在各坐标系下的齐次坐标转换实现对目标的定位。乔川等人［9］基于数字高程模型对地面目标进行定位，以降低目标大地高误差对定位精度的影响。D.Blake Barber等人［10］提出了一种固定翼微型无人机（MAV）在成像时确定地面目标地理位置的方法。利用目标在图像中的像素位置，配合MAV位置和姿态的测量，以及相机摆放角度，将目标定位在WGS-84大地坐标系下。Springer USV使用同步定位与建图（SLAM）技术感知周围目标 环 境［11］。Huntsberger和 Aghazarian 等人［12］研究了一种基于双目立体视觉的USV自主导航研究，通过检测水面障碍物目标，生成危险避障网格图和离散目标列表来实现目标定位自主避障。Gianpaolo Conte等人［13］开发了一种基于视觉的无人机导航系统，利用图像配准技术，从航空图像中提取有用的目标位置信息。

综上所述，USV在自主航行时对目标的感知、识别、定位，多是基于视觉、常规雷达、激光雷达、声呐等传感器来完成［14］。由于单一传感器的感知性能不足，常采用基于多传感器的感知测量系统，实现各种传感器的优势互补［15］。在机器人和无人机（车）领域，主要是对周围目标的定位，无法实时感知运动目标的方位角和速度。而且有些采用稳定可靠的激光雷达SLAM（Simultaneous Localization and Mapping）技术对周围环境进行感知和定位，但该技术主要解决系统本身与环境目标之间的相对空间位置关系，受雷达探测范围的限制，且安装成本较高［15］。因此，本文设计了一种集红外相机、可见光CCD相机、激光测距仪和GPS/INS组合导航系统为一体的目标方位测量系统，并结合传感器组件的输出信息与坐标转换理论，建立了环境目标方位测量模型。该系统既能感知和识别周围环境目标，又能获取远距离目标的位置（相对位置和绝对位置）、方位角和俯仰角、绝对速度。该研究成果将为USV在侦察与监视、海洋测绘、海上搜救等方面的应用奠定理论基础。

1 目标方位测量系统组成及工作原理

1.1 组成原理

USV目标方位测量系统基本组成原理如图1所示，系统主要由转台系统、传感器组件、电子学系统三个部分构成。主要光电器件集成在球形二维转台内，球形转台在方位与俯仰方向上转动，克服无人船在行进过程中的水面波动对扫描或跟踪的影响，从而使系统的光学视轴始终处于水平面内，同时输出转台在执行扫描或跟踪过程中的转台转动的方位角与俯仰角值。传感器组件主要由红外传感器、可见光相机、激光测距机、GPS/INS组合导航定位系统组成。电子学系统主要完成转台伺服控制、遥感温度处理、图像处理、测距计算、目标特性计算等功能。

图1 目标方位测量系统组成原理

1.2 工作原理

无人船目标方位测量系统工作原理如图2所示。红外传感器与可见光相机获得的光学视场内的环境目标，经过图像处理与图像融合后完成目标识别。选定特定目标进行跟踪，通过转台连续在方位与俯仰上的调整使跟踪目标保持在光学视场中心。通过激光测距机完成对跟踪目标距离测量，GPS/INS组合导航系统输出无人船体自身位置、姿态参数。将转台实时调整角度αi、βi，激光测距数据Mi，GPS/INS 组合系统输出的纬度Bi、经度Li、高程Hi，航向角yi、俯仰角pi、横滚角ri进行建模计算（符号中i表示同一时刻），得到被跟踪目标的相对位置与绝对位置特性参数，如果被跟踪的目标是运动目标，通过对目标连续跟踪可以获得被跟踪目标的速度与方位角参数。

图2 目标方位测量系统工作原理

2 目标方位解算

2.1 传感器组件参数坐标系关系

当无人船在自主航行过程中，一旦在光学视场中发现静止或运动目标，需要获得目标的位置、速度、姿态角（主要是方位角）等特性参数，以便指导其合理避障或路径规划，实际用于捕获跟踪目标的光电吊舱系统如图3（a）所示。目标的位置、速度、航向等参数在坐标系中进行描述。因此，需要将传感器组件获得的参数信息与相应的坐标系结合到一起进行特性参数解算。传感器参数与坐标系统之间关系如图3（b）所示。

图3 系统实物图以及传感器组件输出参数与坐标系统之间关系

由图3（b）描述出目标点Pi(i=1,2)与传感器组件输出参数及坐标系统之间的关系。图中用到大地直角坐标系OeXeYeZe、地理坐标系OnXnYnZn、载体坐标系ObXbYbZb。On系与Ob系坐标原点重合，通过GPS获得USV在Oe系下的坐标，On系与Ob系三个坐标轴之间的夹角分布为航向角y、俯仰角p、横滚角r，三个角度值由GPS/INS系统联合给出。M表示无人船原点与目标点Pi(i=1,2)之间的距离，由激光测距机给出。二维转台对目标跟踪时目标相对于Ob系（视轴坐标系与载体坐标系重合）在俯仰上转角为β，在方位方向上为α，通过转台的角度传感器给出。将上述参数与坐标系统结合到一起，解算目标Pi(i=1,2)的相对坐标值（在Ob系下）、绝对位置、绝对速度（动目标）、方位角（在Oe系下，动目标时连续测量两个运动点P1和P2，P1和P2的连线方向表示运动目标的航向方向）。

2.2 目标方位解算原理

（1）坐标系统

根据图3（b）所示，目标方位解算时用到的坐标系统如下：

WGS-84大地坐标系：Oe系（Oe-XeYeZe）：WGS-84坐标系原点是地球的质心，X轴指向零子午面与赤道的交点，Z轴指向1984年规定的协议地球北极方向，Y轴与X轴、Z轴构成右手坐标系。

地理坐标系：On系（On-XnYnZn）：地理坐标系用来表示载体所在位置，原点On一般取在载体的质心，Zn轴沿地垂线指向天，Xn轴指向东方，Yn轴指向北方。它随载体运动而相对地心惯性坐标系运动。

载体坐标系：Ob系（Ob-XbYbZb）载体坐标系是与载体固连的右手直角坐标系，其坐标原点与地理坐标系原点重合，当载体坐标系相对地理坐标系的三轴姿态角为零时，其三轴指向与地理坐标系三轴指向重合。Yb轴为载体纵轴并指向前进方向；Xb轴沿载体横轴向右，即指向前进路线的右方。Zb轴垂直于Xb轴、Yb轴并构成右手坐标系。

在系统设置时使视轴方向与载体坐标系的Y轴方向一致，即二维转台的坐标系与载体坐标系重合，因此省略了一个坐标转换过程，如果使用中二者不重合，需要采取相应措施。目标方位测量技术所采用的坐标转换过程如图4所示。

图4 坐标转换过程

（2）目标位置与速度解算原理

目标位置解算：以无人船艇为载体坐标系的原点，由激光测距机探测距离M和二维转台角度传感器输出的方位角α和俯仰角β计算出目标P1坐标：

式中，(x1,y1,z1)为目标在载体坐标系下位置坐标，根据坐标转换，可求得目标P1在WGS-84大地坐标系下直角坐标(X1,Y1,Z1)，即为绝对坐标值，计算公式如下：

式中，(Xb,Yb,Zb)为船体自身位置坐标；Cnb为载体坐标系到地理坐标系转换矩阵；Cen为地理坐标系到WGS-84坐标系转换矩阵。

其中：

式中，y为船体的航向角；p为船体的俯仰角；r为船体的横滚角。三个角度值由GPS/INS联合给出。

式中，Bb、Lb为On(Ob)系坐标原点的值，由GPS获得。

目标与USV之间的距离解算：目标坐标(Xi,Yi,Zi)(i=1,2,3)，USV船体自身位置坐标(Xb,Yb,Zb)，其两者距离理论值M′，公式如下：

运动目标速度解算：如图3（b）所示，如果P1点是运动目标，通过公式（2）给出P1点在WGS-84坐标系下坐标(X1,Y1,Z1)，当系统处于跟踪状态下，通过对P1点进行连续跟踪，当P1点运动到P2点时，根据公式（2）获得P2点坐标(X2,Y2,Z2)，分别记录下时刻t1与t2，在三个坐标轴上的速度表示为：

则船体速度表示为：

（3）运动目标姿态解算

在图 3（b）中，P1与P2连线构成基线P12，表示运动目标的行进方向，将P2点转换到以根据P1为坐标原点的地理坐标系下坐标(x2,y2,z2)，可以求得运动目标姿态角，即方位角与俯仰角。解算过程如下：

式中，B1、L1为P1与点的纬度与经度值，通过下式获得：

则P12基线所表示运动目标的方位角α12和俯仰角β12分别为：

通过上述过程实现环境目标位置、速度、姿态（方位角、俯仰角）信息解算。

3 实验测试及误差分析

由于实验条件的限制，根据系统的结构提出了半实物仿真方案，采用距系统1 km范围内的两个静止点目标当作移动靶船的起始点，进行实验测试。实验方案如图5所示。

图5 实验方案

实验前，以目标方位测量系统进行数据采集，测试结果与收集到的参数信息进行MATLAB误差仿真分析。测量系统从起点P1到终点P2对目标进行跟踪定位，并在P2点对目标进行反复测量。最终，对系统的相关功能进行了测试，并获取了目标参数和指标。

3.1 测试条件及输入参数

图5中目标方位测量系统放置在固定位置点处，其自身坐标及姿态信息通过GPS/INS组合导航系统测出，并进行视轴标校，使组合导航系统的坐标轴系与目标方位测量系统的坐标轴系重合，降低由于坐标系偏差而引入的系统误差。在距离方位测量系统600～1 000 m位置处使用GPS系统测量出目标的起点P1和终点位置P2。测量的坐标点信息如表1所示，系统初始姿态角（y，p，r）分别为 5.13°、0.25°、0.87°。

根据目标方位测量系统设计，其所搭载传感器的性能指标如表2所示。

由于模拟静态实验，测试过程中，目标方位测量系统的视轴起始方向设置在真北（正北）方向上。用GPS测量目标P1和P2点的实际位置和姿态角，且目标方位测量系统对P2点测量300次，P2点相对于测量系统的位置信息分别为方位角α=16.37°，俯 仰 角β=-0.56°，测距 距离M=971.5m。最后将实验测量值与实际值进行比较，得出系统测量误差。

3.2 实验结果和分析

根据系统测量原理和上述实验方案，首先对目标的终点P2位置进行300次测量，分析目标的定位误差。然后，系统对起始点P1和终点P2进行多次测量，实时获得30组目标方位角和仰角数据，并与目标的实际姿态角数据进行比较。分析得出了目标方位测量系统的方位角和俯仰角误差。

（1）目标位置测量实验

系统在相同位置和姿态下多次测量P2点，测试结果如图6所示。

从实验结果可以看出，由于存在各种误差干扰源，测量值与真实值之间存在误差。对测量数据进行RMSE（均方根误差）统计分析，可以得出，系统测量目标的经度、纬度、大地高误差分布均服从均值μ=0的正态分布，且纬度误差为2.35 × 10-5°，经度误差为3.76 × 10-5°，大地高误差为2.85 m。转换为空间坐标误差，X轴方向误差为3.061 2 m，Y轴为2.725 4 m，Z轴为2.711 5 m。最终位置误差为4.914 4 m。因此，目标位置的测量精度为5 m。

（2）姿态测量实验

当对运动目标姿态测量时，实际上是对目标的体轴线进行测量，从而获得方位角与俯仰角信息，将目标P1位置与P2位置构成基线（该基线可以反映运动目标姿态信息）模拟运动目标。运动目标方位误差∆α与俯仰误差∆β如图7所示。

图7 姿态角误差

对图7中的数据进行统计处理，得到∆α=0.2710°，∆β=0.2497°。可以看出，系统的总体姿态角测量误差为0.3°。实际上，方位角是测量目标姿态时所获得的主要信息。该系统的方位角测量精度约为0.3°，能够满足USV对运动目标姿态的感知要求。

3.3 误差影响因素分析

由式（1）至式（5），目标方位解算模型可以看出，目标位置与姿态测量精度同USV位置、USV姿态、目标相对USV的方位角、俯仰角、距离值有关，也受GPS误差、IMU姿态传感器误差、光电吊舱转台角度传感器误差、激光测距误差的综合影响。为了得出各传感器参数对目标方位测量精度的影响，仅需在其他参数及其误差都不变的情况下，改变其中一个参数的误差值，代入目标位置解算模型式（2）和姿态解算模型式（9），然后采用蒙特卡罗方法建立误差模型，最后通过MATLAB仿真计算得出误差的变化趋势。

蒙特卡罗法的数学模型为：

其中，xi为目标方位解算过程中各参数的测量值；∆xi为其测量误差；F为定位/定姿计算过程。

其目标位置过程可写为：

其中，X表示目标位置解算过程的参数，包括：Bb、Lb、Hb、α、β、M、y、p、r。

由式（13）和式（14）得出位置测量误差模型：

其中，∆B、∆L、∆H为位置测量结果误差；∆X为各参数的误差，且∆X服从均值为0，方差为σ∆X的正态分布。目标姿态解算是根据目标在不同时刻的位置结果而得出的，所以由位置结果代入式（9）求出误差变化即可。

（1）USV位置误差对目标定位定姿精度的影响

在表1参数信息，以及系统初始姿态角y、p、r（5.13°、0.25°、0.87°）条件下，设置 USV 船载 GPS的误差范围为0～5 m，即经纬度误差在0～0.6×10-4°，其余参数的误差不变，如表2所示。如图8（a）和8（b）所示，目标定位定姿的精度随着USV位置误差的增大而呈逐渐上升趋势，目标的经纬度误差影响较小，而大地高误差从2 m增至5 m。

图8 目标位置和姿态测量精度随USV位置误差的变化

（2）USV姿态误差对目标定位定姿精度的影响

其他参数初始值及误差均不变，将USV姿态角的误差从 0°增至 0.2°，仿真结果如图 9（a）和图9（b）所示，位置测量误差从4 m增至11 m。USV自主航行时姿态角数值变化并不大，而姿态角误差对目标定位定姿精度影响却较为严重，所以实际应用中需采用精度比较高的IMU姿态传感器。并且，USV三个姿态角误差和光电吊舱转台角度传感器的方位角误差、俯仰角误差对目标定位定姿精度的影响类似，这是因为两者共同决定了光电吊舱的视轴指向角误差。

图9 目标位置和姿态测量精度随USV姿态误差的变化

（3）目标相对USV的距离对目标定位定姿精度的影响

目标相对USV的距离与USV的大地高这两个参数值对目标定位定姿误差的影响较为相似，且有明显的变化，但两者参数的误差变化对目标定位定姿影响较小，此处不做讨论。设其他参数初始值及误差均不变，将相对距离从600 m增至1 200 m，其误差变化仿真结果如图10（a）和图10（b）所示。其位置测量误差与这两个参数均近似地成正比关系，误差随相对距离的增大而增大。

图10 目标位置和姿态测量精度随相对距离的变化

由于对目标定位的误差增大，运动目标在时间间隔内的起止点构成的目标姿态基线越长，则目标姿态测量误差越大。目标姿态测量精度受相对USV距离和目标起止点距离影响较深，距离越大误差越大。

上文分析了各传感器参数值及其误差值对目标方位测量精度的影响，且各参数误差是相互独立的零均值正态分布。综合分析USV船载GPS精度导致的误差σ1、IMU姿态传感器误差σ2、光电吊舱转台角度传感器误差σ3、激光测距误差σ4对目标方位测量误差的影响，误差计算方法采用均方根法，如式（16）所示。其最终误差仿真结果为：目标位置测量误差5.549 m；目标姿态测量误差0.403°。实际系统测试结果在此误差范围内，因此该目标方位测量系统可满足实际需求，在后续工作中可对其进一步进行滤波优化升级。

4 结论

本文以无人船目标方位测量技术为背景，研究了目标特性参数的获取求解。首先，给出目标方位测量系统整体组成及工作原理，然后，建立动态目标特性参数测量模型，最后，搭建模拟实验系统对功能及系统性能指标进行测试，并进行了误差分析。结果表明系统对目标位置测量精度为5 m，对运动目标姿态角测量精度0.3°，满足无人船自主航行时对目标位置及姿态测量的要求，且为后续无人船自主避障、路径规划提供有效数据支撑和技术支持。在实际应用中，提高自身定位定姿精度，将使系统整体感知精度提高，例如，自身使用差分GPS获得位置信息。下一步将研究如何实现无人船自主航行时，对水面多个目标同时进行实时精确的定位定姿。