水面无人艇环形轨迹跟踪方法研究与实现

宋吉广，李德隆，林 扬，刘肖宇，谷海涛

(1. 中国科学院沈阳自动化研究所 机器人学国家重点实验室，辽宁 沈阳 110016；2. 中国科学院 机器人与智能制造创新研究院，辽宁 沈阳 110169)

0 引 言

水面/水下自主航行器，作为搭载探测仪器与工程设备的自主航行载体，可有效地辅助人类进行海洋数据采集、资源探测和海洋工程实施，对海洋资源探索技术的发展具有重大意义[1-3]。良好的航行控制能力是实现不同需求任务的根本，而对典型航迹的跟踪精度是航行控制效果的良好体现。而目前在USV轨迹跟踪控制的研究中，主要存在以下难点：1）USV系统水动力系数随USV航行速度的变化而改变，因此该系统具有不确定性、时变性和非线性等特点；2）USV具有明显的欠驱动特性，因此很多非线性控制算法不能直接进行应用；3）目前所研究的轨迹跟踪方法多数为基于仿真或无人艇低速状态下的试验，却无法确保在复杂环境、无人艇高速状态下对轨迹的精确跟踪[4]。

无人艇对典型轨迹直线和环形的跟踪能力，是其执行任务应当具备的基本能力。环形轨迹对无人艇的航行控制方法具有较强的能力要求，本文针对环形轨迹跟踪方法进行研究及实现。由于系统模型的不确定性，选用基于拟合偏差的最小二乘航向辨识法建立模型集，实时计算选择最优模型参与控制计算，并以此搭建仿真平台对航向控制、控制策略及制导律等进行设计仿真。本文方法以USV航向控制为核心，结合控制策略的强抗干扰性，以“勇士”号无人水面艇为验证载体，对算法有效性和适用性进行实航验证。

1 基于拟合偏差的最小二乘航向辨识法

“勇士”号USV航向和速度控制执行机构分别为尾部喷嘴和喷泵，通过尾部喷嘴的左右调节改变航向，喷泵向后喷水产生反作用力控制纵向速度。

USV系统航向控制模型采用一般的船舶首摇响应线性方程[5]：

其中：T1，T2，T3为应舵指数参数；K为回转性能参数；r为航向角速度；δ为喷嘴角度。

1957年野本谦使用自动调节原理方法分析式（1），在执行操作非频繁情况下，略去2阶以上的小量，并设T1+T2－KT3=T，基于此简化可近似由下式代替[6-7]：

式中：K为回转性能指数，T为应舵指数，r为航向角速度，δ为喷嘴角度。由式（2）得：

式中，a和b为待估计参数，由此建立USV航向控制模型，设航向角为ψ，如下式：

基于S形开环试验法，获取辨识所需数据，根据最小二乘法辨识参数[8]，构建USV SIMO（sigle input multiple output）模型。设辨识模型为：

式中：U(k) ，y1(k+1)，y2(k) 分 别为第k次输入、输出序列。则其最小二乘格式为：

根据极大值原理，则其取最小值时，应满足

输入用于参数验证的数据，由原值YiM和辨识输出值 Φ获 得 拟 合 偏 差 为：

选择多组数据进行辨识，所得结果在取得拟合偏差后，计算多组结果偏差均值得到阈值将大于阈值的模型剔除并构建模型集，用于运动控制的最优调用。

根据以上原理，通过对“勇士”号USV实航数据进行模型辨识，得到多组模型后选择低于阈值的模型进行验证，结果如图1所示。

图1 实际曲线与拟合曲线对比Fig. 1 Comparison between the fitting curve and practical curve

经过多组数据验证，算法拟合输出与实际输入基本吻合，且其平均拟合偏差较小，符合构建模型条件。

2 势点跟踪算法原理

设定环形轨迹中心和半径，输入当前USV的位置、速度、航向信息，完成势点的划分、首势点选择及环绕方向判定，实时将有序势点进行坐标转换，以获得USV在所跟踪势点的运动坐标系下坐标，通过势点切换判据更新所跟踪的势点，并实时计算期望航向角。根据USV辨识模型所设计的控制策略对航向进行控制，实现对势点的稳定跟踪[9-11]。算法实现流程如图2所示。

图2 势点跟踪算法实现流程Fig. 2 Implementation process of potential point tracking algorithm

根据水面无人艇动力学模型及运动学模型，将系统航向控制模型式（4）转换为状态空间表达式[12]：

首先，预设闭环系统矩阵极点为λ1，λ2， 且－1＜λ1,λ2＜0，根据现代控制理论中极点配置问题，构建动态反馈控制律[13]：

其中，a和b为辨识所获得参数。

2.1 势点跟踪策略设计

依托于水平面航向控制器[14-16]，采用势点跟踪算法实现环形轨迹跟踪，如图3所示。首先设定环形轨迹中心位置以及环形半径，根据中心位置建立运动坐标系，并预置有序势点集合。对航行器位置进行坐标变换，转至运动坐标系下，依次跟踪每一个势点，进而形成环形轨迹。

图3 势点跟踪算法基本原理图Fig. 3 Basic schematic diagram of potential point tracking algorithm

根据轨迹半径、航行速度，计算势点数量，并将所得到的势点存入Paim[]中：

图4 势点切换判据原理图Fig. 4 Principle diagram of potential point switching criterion

当满足如下切换条件，切换至下一势点继续跟踪。

2.2 制导律设计

基于视线导引方法设计路径跟随制导系统，期望路径由一系列点Paim[]=[Paim[1],···,Paim[n]]组成。设Paim[t]=(xnt,ynt) ，艇位置 (xt,yt) ，d为艇中心到直线Paim[t]Paim[t+1]的距离，即制导系统使该值趋于0。β为侧漂角，ψ为USV航向角，ψpt为直线与北向夹角角度，Rt为收敛半径，一般为艇长的2～3倍，Δ为超前距离，ψd为视线角[17-19]，如图5所示。

图5 航向控制计算原理图Fig. 5 Course control calculation schematic digram

其中：

则可求横向距离d、超前距离Δ 和视线角ψd，

由此可得USV航向期望角度为：

当横向误差d绝对值大于Rt时，制导方法失效，故Rt应实时适应d变化，并使d快速收敛，则

通过调节参正数μ 以 保证 |Rt|＞|d|，同时，横向误差d增大，视线角ψd增大，d将快速收敛，横向误差d减小，视线角ψd减 小，d将较慢收敛，轨迹将趋于圆滑。

为减小因水流干扰产生的稳态误差，计算视线角时将横向误差进行积分，其方式为：

其中，ki为积分参数，所以轨迹跟踪制导律为：

该无人艇制导律可通过航向调节快速有效的收敛到环形轨迹上，同时，消除外界干扰导致的稳态误差，增强系统的鲁棒性[20]。

3 实验仿真及外场试验

经由辨识方法得到的USV数学模型，搭建仿真平台，在此平台下进行多工况环形轨迹跟踪方法验证。设定环形轨迹半径100 m，USV速度9 m/s，进行环形轨迹跟踪。仿真结果如图6所示：

图6 仿真平台下的环形轨迹跟踪Fig. 6 Circular trajectory tracking in simulation platform

由图6（b）中间距稳定状态可知，USV对环形轨迹的跟踪效果良好。经过计算，在该仿真状态下其实航轨迹RMS为0.8 m，具备良好的轨迹跟踪能力。

为提高算法的实用性，以“勇士”号USV为验证平台，进行典型海况下（3级海况）海上环形轨迹跟踪多条次试验。设定环形轨迹半径为100，航行速度为11 m/s，USV对所设定的环形轨迹进行跟踪，经对数据整合处理，其实航轨迹效果如图7所示。

在海上环境中，由于风浪流等环境干扰，将对环形轨迹跟踪方法的抗干扰能力进行检验。由图7（b）可知，USV按照所设计的势点跟踪算法及制导律进行运行，其能够按照预定航向、航迹航行，且通过对实航数据的计算得出实航轨迹RMS为1.2 m，表明环形轨迹跟踪方法具有较强的抗干扰能力，即在较强环境干扰、高速状态下亦可实现对环形轨迹的稳定跟踪。

图7 海试中环形轨迹跟踪Fig. 7 Circular trajectory tracking in sea trial

4 结 语

本文针对USV在实航中模型参数不确定、高速状态下对环形轨迹的准确跟踪能力方面研究薄弱问题，提出了基于势点跟踪算法的环形轨迹跟踪方法。算法以“勇士”号USV作为验证平台，经过模型参数辨识得到系统数学模型集，进而建立USV仿真平台，基于系统模型辨识构建跟踪策略以及制导律，对环形轨迹跟踪方法进行仿真验证，并在典型海况下多条次环形轨迹跟踪试验进行验证。实航数据中轨迹的RMS较小，可良好的在环境干扰、高速状态下实现对环形轨迹的精确跟踪，表明在USV平台下该方法对环形轨迹稳定跟踪的有效性、适用性以及鲁棒性。