0 引　言

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

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

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

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

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

$ {T_1}{T_2}\ddot r + \left( {{T_1} + {T_2}} \right)\dot r + r = K\delta + K{T_3}\dot \delta {\text{。}} $

(1)

其中： ${T_1}$ ， ${T_2}$ ， ${T_3}$ 为应舵指数参数； $K$ 为回转性能参数； $r$ 为航向角速度； $\delta $ 为喷嘴角度。

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

$ T\dot r + r = K\delta {\text{。}} $

(2)

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

$ \dot r = - \frac{1}{T}r + \frac{K}{T}\delta = ar + b\delta \text{，} $

(3)

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

$ \left\{ \begin{split} \dot \psi &= r\text{，} \\ \dot r &= ar + b\delta \text{，}\\ \end{split} \right. $

(4)

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

$ {y_1}\left( {k + 1} \right) = a{y_2}\left( k \right) + bU\left( k \right) + e\left( k \right)\text{，} $

(5)

式中： $U\left( k \right)$ ， ${y_1}\left( {k + 1} \right)$ ， ${y_2}\left( k \right)$ 分别为第 $k$ 次输入、输出序列。则其最小二乘格式为：

$ \left\{ \begin{split} \varphi \left( k \right) = {\left[ {\begin{array}{*{20}{c}} {{y_2}\left( k \right)}&{U\left( k \right)} \end{array}} \right]^{\rm{T}}} \text{，} \\ \theta = {\left[ {\begin{array}{*{20}{c}} a&b \end{array}} \right]^{\rm{T}}} \text{，}\qquad\qquad\;\; \\ \end{split} \right. $

(6)

辨识模型即为：

$ y\left( {k + 1} \right) = \varphi {\left( k \right)^{\rm{T}}}\theta \left( k \right) + e\left( k \right){\text{。}} $

(7)

设采样数组数据量为 $M$ 时，该辨识系统中 $\left\{ {\begin{array}{*{20}{l}} {{\varPhi _M} = \left[ {\varphi \left( k \right),\varphi \left( {k + 1} \right), \cdots ,\varphi \left( {k + M - 1} \right)} \right]} \\ {{Y_M} = \left[ {{y_1}\left( k \right),{y_1}\left( {k + 1} \right), \cdots ,{y_1}\left( {k + M - 1} \right)} \right]} \\ {{e_M} = \left[ {e\left( k \right),e\left( {k + 1} \right), \cdots ,e\left( {k + M - 1} \right)} \right]} \end{array}} \right.$ 分为第 $M$ 次输入、输出观测数据及误差数据，引入最小二乘准则函数：

$ J = \sum\limits_{i = 0}^{M - 1} {{{\left( {{y_1}\left( {k + 1 - i} \right) - {\varphi ^{\rm{T}}}\left( {k + i} \right)\tilde \theta } \right)}^2}} \text{，} $

(8)

根据极大值原理，则其取最小值时，应满足 $\dfrac{{\partial J}}{{\partial \tilde \theta }} = 0$ 存在，即

$ - \left( {\Phi _M^{\rm{T}}\left( {{Y_M} - {\Phi _M}\tilde \theta } \right) + \Phi _M^{\rm{T}}\left( {{Y_M} - \Phi _M^{\rm{T}}\tilde \theta } \right)} \right) = 0\text{，}$

(9)

式中，若矩阵 $\Phi _M^{\rm{T}}{\Phi _M}$ 可逆，则由此获得最小二乘估计值为：

$ {\tilde \theta _M} = {\left( {\varPhi _M^{\rm{T}}{\Phi _M}} \right)^{ - 1}}\varPhi _M^{\rm{T}}{Y_M}{\text{。}} $

(10)

输入用于参数验证的数据，由原值 ${Y_{iM}}$ 和辨识输出值 $\Phi _{iM}^{\rm{T}}{\tilde \theta _j}$ 获得拟合偏差为：

$ {\bar e_{jM}} = \frac{{\left( {{Y_{iM}} - \Phi _{iM}^{\rm{T}}{{\tilde \theta }_j}} \right)}}{M}\text{。} $

(11)

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

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

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

2 势点跟踪算法原理

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

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

$ \left[ \begin{array}{l} {\dot \psi } \\ {\dot r} \\ \end{array} \right] = \left[ {\begin{array}{*{20}{c}} 0&1 \\ 0&a \end{array}} \right]\left[ \begin{array}{l} \psi \\ r \\ \end{array} \right] + \left[ \begin{array}{l} 0 \\ b \\ \end{array} \right]{\delta _{\rm{r}}}\text{，} $

(12)

首先，预设闭环系统矩阵极点为 ${\lambda _1}$ ， ${\lambda _2}$ ，且 $ - 1 < $ $ {\lambda _1}, {\lambda _2} < 0$ ，根据现代控制理论中极点配置问题，构建动态反馈控制律^[13]：

$ {\delta _{\rm{r}}}\left( t \right) = \frac{{{\lambda _1} + {\lambda _2} + a}}{b}r - \frac{{{\lambda _1}{\lambda _2}}}{b}\psi \text{。} $

(13)

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

2.1 势点跟踪策略设计

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

根据轨迹半径、航行速度，计算势点数量，并将所得到的势点存入 ${P_{{\rm{aim}}}}[]$ 中：

$\beta = \alpha {\text{π}} \cdot {\left( {floor\left( {2{\text{π}} R \cdot {{\left( {20v{d_{\rm{f}}}} \right)}^{ - 1}}} \right) + 100} \right)^{ - 1}}{\text{，}}$

(14)

$ n = floor\left( {\frac{{2 \cdot {\text{π}} }}{\varsigma }} \right){\text{。}} $

(15)

式中： $R$ 为轨迹半径； $v$ 航行速度； ${d_{\rm{f}}}$ 设定参数； $\alpha $ 设定参数； $\varsigma $ 为每2个势点所对应扇形的圆心角。

在大地坐标系下，根据当前跟踪势点 ${P_{{\rm{aim}}}}[a]$ 位置与航行器位置，实时求解航行器的期望航向角，直至完成对当前势点的跟踪后切换至下个势点，继续跟踪。航行器与势点位置关系如图4所示，由当前势点 $ {P}_{\rm{aim}}\left[a\right] $ 建立坐标系， $\overrightarrow {O{P_{{\rm{aim}}}}\left[ a \right]} $ 为 ${X^{'}}$ 轴，逆时针方向建立 ${Y^{'}}$ 轴。航行器在此坐标系下位置为 $\left( {{X_{n}},{Y_{n}}} \right)$ ，与势点距离为 $d$ ，门限参数为 ${d_{\rm{m}}}$ ， $ {x}_{0} $ 和 $ {y}_{0} $ 。

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

$\left( {{X_{n}} > {x_0}} \right) \cap \left( {{Y_{n}} > {y_0}} \right) \cup \left( {d > {d_{m}}} \right)\text{。} $

(16)

2.2 制导律设计

基于视线导引方法设计路径跟随制导系统，期望路径由一系列点 ${P_{{\rm{aim}}}}[] = \left[ {{P_{{\rm{aim}}}}[1], \cdots ,{P_{{\rm{aim}}}}[n]} \right]$ 组成。设 ${P_{{\rm{aim}}}}[t] = ({x_{{\rm{nt}}}},{y_{{\rm{nt}}}})$ ，艇位置 $({x_{\rm{t}}},{y_{\rm{t}}})$ ， $d$ 为艇中心到直线 ${P_{{\rm{aim}}}}[t]{P_{{\rm{aim}}}}[t + 1]$ 的距离，即制导系统使该值趋于0。 $\beta $ 为侧漂角， $\psi $ 为USV航向角， ${\psi _{{\rm{pt}}}}$ 为直线与北向夹角角度， ${R_{\rm{t}}}$ 为收敛半径，一般为艇长的2～3倍， $\Delta $ 为超前距离， ${\psi _{\rm{d}}}$ 为视线角^[17-19]，如图5所示。

其中：

$ {\psi _{{\rm{pt}}}} = a\tan 2\left( {{y_{{\rm{nt}} + 1}} - {y_{{\rm{nt}}}},{x_{{\rm{nt}} + 1}} - {x_{{\rm{nt}}}}} \right)\text{，} $

(17)

则可求横向距离 $d$ 、超前距离 $\Delta $ 和视线角 ${\psi _{\rm{d}}}$ ，

$d = \left( {{y_{\rm{t}}} - {y_{{\rm{nt}}}}} \right)\cos \left( {{\psi _{{\rm{pt}}}}} \right) - \left( {{x_{\rm{t}}} - {x_{{\rm{nt}}}}} \right)\sin \left( {{\psi _{{\rm{pt}}}}} \right)\text{，}$

(18)

$ \Delta = \sqrt {{R_{\rm{t}}}^2 - {d^2}} \text{，} $

(19)

$ {\psi _{\rm{d}}} = \arctan \left( { - \frac{d}{\Delta }} \right){\text{。}} $

(20)

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

$ {\psi _{{\rm{ex}}}} = {\psi _{\rm{d}}} + {\psi _{{\rm{pt}}}} - \beta \text{，} $

(21)

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

$ {R_{\rm{t}}} = \frac{\mu }{{\left| d \right| + 1}} + \zeta \left| d \right|,\left( {0 < \zeta < 1} \right){\text{。}} $

(22)

通过调节参正数 $\mu $ 以保证 $\left| {{R_{\rm{t}}}} \right| > \left| d \right|$ ，同时，横向误差 $d$ 增大，视线角 ${\psi _{\rm{d}}}$ 增大， $d$ 将快速收敛，横向误差 $d$ 减小，视线角 ${\psi _{\rm{d}}}$ 减小， $d$ 将较慢收敛，轨迹将趋于圆滑。

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

$ {\psi _{\rm{d}}} = \arctan \left( { - \frac{1}{\Delta }(d + {k_{\rm{i}}}\int {d{\rm{dt}}} )} \right)\text{，} $

(23)

其中， ${k_{\rm{i}}}$ 为积分参数，所以轨迹跟踪制导律为：

${\psi _{{\rm{ex}}}} = \arctan \left( { - \frac{1}{\Delta }(d + {k_{\rm{i}}}\int {d{\rm{dt}}} )} \right) + {\psi _{{\rm{pt}}}} - \beta \text{。}$

(24)

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

3 实验仿真及外场试验

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

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

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

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

4 结　语

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