无人艇航迹跟踪控制系统设计与验证

刘志强， 卞涛,2,*， 叶曦， 刘彬

(1.江汉大学智能制造学院， 武汉 430056； 2.广东华中科技大学工业技术研究院， 东莞 523808；3.华中科技大学人工智能与自动化学院， 武汉 430074)

近年来，无人艇(unmanned surface vehicle，USV)的智能化水平逐步提高，应用领域逐渐扩展。在军事领域，USV承担着反潜侦察、国土安全、情报收集等使命[1-2]；在民用领域，USV在环境检测、水质监测、水上救援等方面得到应用[3-4]。USV依托其小型轻便、成本低、无人化等特点[5]，受到中外学者的广泛关注。无人艇航迹跟踪定义为从任意初始位置出发，在满足空间和时间要求的前提下，实现跟踪或遵循预定航迹的目的。航迹跟踪不仅需要保持正确航向以达到沿预定航线高速行驶，还需克服各种恶劣环境。当前，无人艇航迹跟踪算法的理论研究较多，实际平台搭建较少，而且无人艇系统包含了导航、制导以及控制三个部分，它们之间存在相互作用，每一个部分都直接关系到无人艇执行水面任务的效率和安全。因此，开发一种能够应用于内湖环境中的无人艇航迹跟踪控制系统具有很大的价值和意义。

许多USV装备有两个独立的推进器或一个推进器和一个方向舵，它们属于一类欠驱动系统。这类系统的控制输入数量比自身所需控制的自由度数量少，所以会存在不可积的加速度约束，从而增加了系统的复杂性[6]。另外，为全驱动USV所提出的许多运动控制方案可能不会直接推广到欠驱动USV上，这也为解决欠驱动USV航迹跟踪控制研究带来了困难。

USV的欠驱动问题要求在某种制导策略的作用下，将对USV浪涌、横漂以及首摇三个自由度的运动控制转化为一个航向控制问题。针对这一问题，当前解决策略中一个重要的方法就是视线(line of sight，LOS)制导法，许多学者对此开展了研究。文献[7]将LOS制导算法应用于无人艇直线航迹跟踪，通过实船实验进行了验证；文献[8]设计了一种基于LOS算法和级联定理的路径跟踪控制策略，使得交叉误差均匀半指数稳定，然而导航定律容易受到环境干扰，如风、浪和洋流的影响。如果USV受到环境干扰的影响，会出现收敛问题和路径偏差；为补偿这种干扰，在文献[9]提出了一种考虑外部环境干扰的自适应LOS制导算法，通过仿真分析验证了算法收敛速度快，路径跟踪误差小，然而未将无人艇应用于室外内湖环境中执行水面任务。总的来说，由于LOS制导算法的简单性和实用性，它在无人艇航迹跟踪控制系统设计中发挥了关键作用。

在无人艇的航向控制问题中，PID控制、滑模控制、神经网络控制等方法的应用已有诸多文献做了研究。文献[10]应用粒子群算法改进PID控制器，设计了无人艇智能航向控制系统；文献[11]将基于指数滑模面的滑模同反演法结合，设计了无人艇航向控制器，通过仿真分析和实物实验验证了算法的有效性；文献[12]基于双体结构的无人艇，利用RBF神经网络改进PID算法，在数值仿真分析中验证了该控制器具有较好的性能。虽然，智能算法结合PID控制能够大幅提高控制性能，但可能会遇到复杂的计算问题，在实际应用中难以实现。另外，精确的无人艇数学模型是难以建立的，致使许多控制算法在实际工程应用中效果欠佳。近年来，无模型自适应控制方法在工业控制领域得到了广泛应用[13-14]，它是一种数据驱动控制，具有自适应特性，更适合处理系统的不确定性[15]。由于无模型自适应控制具有计算简单、鲁棒性强、模型要求低等优点，它更有利于将无人艇航迹控制系统应用到实艇中。

综合上述分析，目前中外在无人艇航迹跟踪控制上所取得的研究成果大多处于数值仿真或者半物理仿真阶段，实际应用较少。因此，本文的研究目标是设计一个由控制算法、数据采集与处理、上位机监控三部分组成的无人艇航迹跟踪控制系统，实现无人艇在内湖稳定地执行水面任务。首先，在数据采集与处理部分采用卡尔曼滤波算法；其次，在控制算法部分设计基于自适应LOS算法的制导系统和基于无模型自适应PID算法的航向控制系统；然后，为实时获取无人艇航行的位置和姿态信息，开发一种上位机监控系统；最后，基于“江豚-16”全自主无人艇开放实验平台测试航迹跟踪控制系统的效果，在室外内湖环境下进行实艇验证。

1 无人艇系统总体结构

1.1 系统模型

以江汉大学“江豚-16”无人艇实验平台为研究对象，设计航迹跟踪控制系统以实现无人艇的自主航行。该艇长1.6 m，最大载荷35 kg，配备有喷水推进器、直流无刷电机、倒车舵机、差分GPS等，同时搭配多种传感器，包括激光雷达、惯导、视觉等。其外部结构如图1所示。

图1 无人艇外部结构Fig.1 USV external structure

“江豚-16”无人艇的数学模型为

(1)

式(1)中：

(2)

(3)

(4)

(5)

(6)

(7)

式中：x、y和ψ分别为惯性坐标系下无人艇的位置和方向；u、v和r分别为艇体坐标系下的前进速度、横漂速度和航向角速度；τu和τr分别为前进力和首摇力矩；τwind和τwave分别为风、浪引起的力和力矩。

图3 系统整体框架Fig.3 Overall system framework

1.2 运动控制系统总体设计方案

图2 无人艇运动控制系统总体结构Fig.2 Overall structure of USV motion control system

无人艇运动控制系统总体分为三个部分，分别是任务层、控制层和执行层，如图2所示。在各层协同下控制无人艇自主航行，当遭遇突发状况或不明障碍物时，任务层可以直接对执行层发送控制指令，从而避免系统响应的延迟。控制层根据任务层所获取到的目标指令以及环境信息，采用一些算法来求解出应施加在无人艇各个自由度上执行机构的推力和力矩，再将这部分指令发送到执行层。执行层又根据控制层所获取到的信息将推力和力矩合理施加到执行机构上，通过驱动执行机构来作出相应的运动，从而让无人艇按照预设航迹自主航行。

2 航迹跟踪控制系统设计

本文研究中开发了一种无人艇航迹跟踪控制系统，如图3所示。该系统分为航迹跟踪、数据采集与处理、上位机监控三个子系统。在航迹跟踪子系统中，搭建了基于自适应LOS制导算法和无模型自适应PID的航迹控制器；在数据采集与处理子系统中，将采集到的导航信息和状态环境信息经过卡尔曼滤波器融合处理；在上位机监控子系统中，根据用户指令设定目标航迹，无人艇跟踪目标航迹的位置信息和状态信息被实时监控和保存。

2.1 数据采集与处理

无人艇航迹跟踪控制系统的数据采集与处理任务主要是获取全部传感器数据信息，包括无人艇的位置、姿态、速度、舵角、转速以及各种障碍物等信息。为准确估计无人艇航行过程中的位置和姿态信息，本文采用卡尔曼滤波器对传感器信息进行融合处理。

将卡尔曼滤波器的输入作为组合导航系统的量测值，输出作为惯导系统的估计误差值，其计算步骤如下。

首先定义状态预测方程和预测方差阵[16]为

Xk,k-1=Φk,k-1Xk-1

(8)

(9)

式中：Xk,k-1为前一个状态的预测值；Xk-1为前一个状态的最优值；Φk,k-1和Ek-1为系统参数；Mk,k-1为对应Xk,k-1的协方差；Mk-1为对应Xk-1的协方差；Nk-1为系统过程噪声的协方差。

然后对系统状态变量进行估计，如式(10)所示，以Xk作为当前最优状态估计值，Zk为系统的测量值。

Xk=Xk,k-1+Kk[Zk-HkXk,k-1]

(10)

由式(7)和式(8)计算卡尔曼增益矩阵Kk：

(11)

式(11)中：Hk为量测矩阵；Rk为测量噪声的协方差。

最后估计均方差矩阵Mk，有

ΜMk=(I-KkHk)Mk,k-1

(12)

2.2 LOS制导系统

图4 LOS制导算法航迹跟踪原理Fig.4 LOS guidance algorithm track tracking principle

无人艇航迹跟踪的准确性很大程度上取决于制导算法，LOS制导算法在水面船舶航迹跟踪控制中应用广泛。其原理是在预先设定的一段航迹上选取视线点来求解无人艇在每个航迹点处的期望航向角[17-19]。在制导系统中，采用自适应LOS算法计算目标转向角，作为航向控制系统的输入。LOS航迹跟踪原理如图4所示。在图4中，Pn点为路径起点，Pn+1点为目标航迹点，O点为无人艇实时位置，以O点为圆心，以可调参数RLOS为可视半径，可得到一片扇形区域。假设无人艇长为L，船长比为n，则R=nL，Δh为前视距离，e为航迹误差。O点正北方向与OPLOS直线之间的夹角为实时目标航向ψr，而实际航向ψ由北斗定位系统获取，ψr-ψ为偏差角度Δψ。当无人艇进入Pn+1点圆形区域内时，将切换到新的航迹点，循环往复，最终实现无人艇对目标航迹的跟踪。

应用式(13)可求得PLOS：

(13)

根据无人艇的当前位置和期望航迹获取目标航向ψr为

(14)

无人艇空间固定速度到惯性速度的转换关系为

(15)

(16)

(17)

式中：e为交叉跟踪误差；浪涌速度假定为常数u0，且u0>0；横漂速度通常很小，可以假定为v≈0。故，式(16)简化为

(18)

(19)

(20)

这里采用Lyapunov第二法证明e收敛到0，设置Lyapunov函数为V(e)=e2，则有

(21)

上述所设计的基本LOS算法的视线圆半径是固定的，在跟踪预设航迹时收敛速度较慢，难以保证较高的跟踪精度。故提出一种自适应LOS算法，基于过渡层自适应地调整视线圆半径，具体方案为

(22)

式(22)中：a=Rmin-μ；b=Rmin+μ；Rmin为最小可视半径；μ为过渡层系数；k3为可调参数。调整方案概述为：当|e|≤a时，无人艇以最小内切圆可视半径Rmin收敛到目标航迹；当|e|>b时，可视半径RLOS为当前航迹误差|e|；当a<|e|≤b时，可视半径RLOS在区间[Rmin,b]内光顺过渡，使得视线角抖动变小，无人艇能够平稳地趋向于目标航迹。

2.3 航向控制系统

采用无模型自适应PID(model free adaptive PID，MFA-PID)算法设计航向控制器，通过北斗定位系统获取无人艇实时位置和航向信息，以LOS制导算法求解的目标航向ψr为输入，控制量为指令舵角δr，系统输出为位置信息，即当前坐标(x,y)和实际航向ψ，从而形成双闭环结构的航迹跟踪系统，如图5所示。

图5 航迹跟踪控制结构Fig.5 Trajectory tracking control structure

无人艇航向控制系统属于SISO非线性离散系统，计算公式[20-22]为

y(k+1)=f[y(k),…,y(k-ny),

u(k),…,u(k-nu)]

(23)

式(23)中：y(k)∈R和u(k)∈R分别为k时刻系统的输入输出；R为实数集；ny和nu为系统的阶次。

对式(23)提出如下假设[23]。

假设1f(…)关于第(ny+2)个变量的偏导数存在且连续。

假设2式(23)满足广义Lipschitz条件，即对任意k1≠k2，k2≥0和u(k1)≠u(k2)有

|y(k1+1)-y(k2+1)|≤b|u(k1)-u(k2)|

(24)

式(24)中：y(ki+1)=f[y(ki),…,y(ki-ny),u(ki),…,u(ki-nu)]；i=1,2；b>0为常数。

假设3是在控制系统的设计中对一般非线性系统设定的一种典型的约束条件；假设2是对系统输出变化率上界的一种限制，也即有界的输入能量变化产生有界的输出能量变化，显然它包括一类非线性系统。

定理1对满足假设1和假设2的非线性系统[式(23)]，当|Δu(k)|≠0时，必存在一个被称为“伪偏导数”的时变参数φ(k)∈R，使得系统[式(23)]可转化为如下CFDL数据模型：

Δy(k+1)=φ(k)Δu(k)

(25)

且φ(k)在任意k时刻有界，Δy(k+1)=y(k+1)-y(k)，Δu(k)=u(k)-u(k-1)。定理1的证明参见文献[24]。

因此，可得到式(23)的动态线性化模型，即泛模型为

y(k+1)-y(k)=φ(k)u(k)-u(k-1)

(26)

根据式(26)进行无模型自适应PID控制律设计。若系统期望输出为yd(k)，系统控制目标是找到一个控制序列u(k)使得在k趋于无穷时航向误差e(k)=yd(k)-y(k)收敛于0。

MFA-PID设计方案如下：

kp[e(k)-e(k-1)]+kie(k)+

kd[e(k)-2e(k-1)+e(k-2)]

(27)

(28)

(29)

2.4 上位机监控系统

为了实时获取无人艇航行过程中的位置和姿态信息，实现对无人艇数据融合处理、任务设定分配以及实时地图显示，建立了一款无人艇航迹跟踪上位机监控系统，系统框架如图6所示。

图6 上位机监控系统框架Fig.6 Framework of upper computer monitoring system

上位机监控系统主要功能如下。

(1)显示电子地图。该软件设计了地图显示区，能够实现2D地图和卫星地图的切换，并添加了谷歌地图应用。地图中可以显示无人艇的航行状态，包括坐标、任务点、航迹等信息，如图7所示。

(2)接收处理无人艇状态数据。由遥控器传输无人艇航行的状态信息，上位机软件通过232总线实时接受。所接受的数据信息会经过卡尔曼滤波器处理，再对数据进行过滤转化。

(3)设置航迹点位置。用户可通过鼠标左键在地图区域设置预定航迹点，第一个航迹点作为起点，最后一个航迹点作为终点。通过任务设置功能将所有航迹点作为任务信息发送给无人艇，点击“主控板运行”，无人艇会按照预设航迹自主航行。

(4)执行航道跟踪任务。在航道跟踪模式下，地图上会显示出预设航迹和无人艇实际航行的航迹，并能实时显示无人艇航行姿态。无人艇在执行航迹跟踪任务时，用户可随时取消任务。

图7 上位机监控界面Fig.7 Upper computermonitoring interface

3 实艇验证

利用所设计的无人艇航迹跟踪控制系统进行实艇实验，以验证该控制系统的有效性和可靠性。实验对象为“江豚-16”无人艇实验平台，实验地点在江汉大学三角湖龙舟码头，实验时天气晴朗，气温18 ℃，风浪较小。制导系统参数设置为：Rmin=3，μ=1，k3=0.02；航向控制系统参数设置为：kp=0.653，ki=0.001，kd=1.125，λ=5，μ=100，ρ=1，η=0.6。图8为实验过程的工作场景，图9为无人艇在湖面的运行情况。本实验分为三组，第一组在不同航速下测试无人艇跟踪直线航迹的效果；第二组在同一航速下测试无人艇跟踪不同轨迹的效果；第三组测试无人艇航迹跟踪控制系统面对随机强干扰情况的效果。通过这三组实验来综合验证无人艇的航迹跟踪能力和系统的实用性。

图8 实验工作场景Fig.8 Experimental working scene

图9 无人艇湖面航行情况Fig.9 USV sailing on the lake

3.1 不同航速下跟踪直线航迹

为验证“江豚-16”无人艇在不同航速下的航迹跟踪能力，设定4种航速测试实验，分别为1、2、3、5 m/s。图10为4种航速下无人艇跟踪直线航迹效果。从图10中可以看出，无人艇能够准确地从航迹点1跟踪行驶到航迹点2。图10(a)～图10(c)反映了无人艇以较低速度航行的情况，跟踪航迹与预设航迹基本吻合，但速度越快，跟踪误差也随之增大。当速度提升到5 m/s时，从图10(d)中可以看到，无人艇明显偏离了预设航迹，跟踪误差较大。这是由于航速太快，算法收敛速度跟不上，另外航向航速具有耦合作用，较快的航速也影响了航向，使得无人艇偏离了预设航迹，但只是增大了误差并未对无人艇的稳定行驶产生太大影响，说明该系统的稳定性还是可靠的。

3.2 同一航速下跟踪不同航迹

无人艇在实际应用中需要在水面到达不同地点，执行各种任务。为验证“江豚-16”无人艇执行不同任务的能力，统一设定航速为1.8 m/s，跟踪任务设置两段航迹、三段航迹、四段航迹、五段航迹，分别对应于4组水面任务。图11为无人艇执行不同水面跟踪任务的情况。从图11中可以看出，无人艇可以顺利完成4种不同情况的水面任务。当任务点出现拐角时，无人艇会产生较大幅度的转向，这个转向角来自LOS制导算法的解算，图中无人艇的转向效果良好，验证了制导系统的合理性，证明本文所设计的无人艇航迹跟踪控制系统具有较好的实际应用价值和意义。

图12 随机强干扰下航迹跟踪Fig.12 Path tracking under random strong interference

3.3 湖面随机强干扰情况

复杂湖面环境下无人艇自主航行不仅会受到风、浪、流等因素的影响，还会遇到水面垃圾、“鱼跃水”以及不明障碍物等随机强干扰情况。应对随机强干扰的能力是评价无人艇航迹跟踪控制系统的重要指标，因此本文研究在湖面反复进行航迹跟踪实验，在多组实验中找出突发随机强干扰的例子，如图12所示。

从图12(b)中可以看到无人艇航迹跟踪轨迹在第一段直线路径上出现了较大偏离，从图12(c)和图12(d)中反映出航向角和航速在31 s左右受到了随机强干扰作用，这一作用使得航向角最大偏离了9°左右，跟踪误差也随之变大。但经过约5 s的调整后，无人艇又回到了稳定地航行状态，并没有对后续的跟踪任务造成太大影响，这说明本文所设计的航迹跟踪控制系统具备应对随机强干扰的能力，具有较好的鲁棒性和稳定性。

4 结论

无人艇作为一类智能机器人，未来在江河湖海上的应用具有极大的潜力。针对无人艇在内湖执行水面任务的难点，设计了一种无人艇航迹跟踪控制系统，并将其应用于“江豚-16”无人艇实验平台。该系统利用卡尔曼滤波器进行数据采集和处理，基于自适应LOS制导算法对预设航迹进行跟踪，通过设计无模型自适应PID航向控制器，保证了航迹跟踪系统的稳定性。在实艇验证过程中，利用“江豚-16”无人艇在江汉大学三角湖水域分别测试了不同航速下跟踪直线航迹、同一航速下跟踪不同航迹以及面对随机强干扰的效果，结果表明，该航迹跟踪控制系统能够实现无人艇在内湖中稳定地执行各种水面任务，跟踪效果良好，具有较强的鲁棒性和实用性。

不过，本文所设计的无人艇航迹跟踪控制系统仅在内湖环境下进行了验证，接下来还需要在河流、海洋等更加复杂多变的环境下进行测试，另外设计考虑漂角、风、浪、流等因素影响的航迹跟踪算法也有待进一步研究。