基于改进的视线导引算法与自抗扰航向控制器的无人艇航迹控制

杨忠凯，仲伟波*,2，冯友兵，孙彬

1 江苏科技大学 电子信息学院，江苏 镇江 212003

2 江苏科技大学 海洋装备研究院，江苏 镇江 212003

0 引 言

无人艇（USV）是一种能够自主航行并完成任务的水面舰艇，在军事作战、维护海域安全、海洋资源勘测、环境监测等领域具有广泛的应用前景[1]。无人艇的运动控制主要包括航速与航向控制、航迹控制、智能规划、避障与导航、编队协同等，其中航迹控制是无人艇能够自主完成任务的前提，也是无人艇智能控制的核心之一[2-3]。

无人艇航迹控制的主要方法有视线导引(line-of-sight, LOS)算法[4]、反步滑模技术[5]、串级PID[6]、神经网络控制[7]、鲁棒自适应控制[8]、模糊控制[9]等。田勇等[10]利用LOS 及抗饱和PID 算法设计并实现了直线航迹跟踪，解决了航向超调和振荡问题，但外界扰动对航向精度和稳定性具有较大影响。陈霄等[11]利用改进的LOS 算法与自适应滑模航向控制算法结合，引入自适应观测器对漂角进行实时估计和补偿，取得了较好的航迹跟踪效果，但在实艇验证的期望航迹拐点处，无人艇航迹跟踪误差较大。祝镜等[12]基于LOS 算法对增量式PID、模糊PID 以及可变船长比模糊PD 这3 种控制算法进行仿真分析，结果表明可变船长比模糊PD 控制更具抗干扰优势。范云生等[13]设计了一种LOS 与模糊自适应PID 结合的航迹控制算法并进行实艇试验，结果表明该算法降低了时变漂角对航迹控制的影响，但在路径拐点处会出现一定的转向超调及较大的位置偏差。Fossen等[14]将非线性积分自适应LOS 算法与PID 航向控制器结合，结果表明该航迹控制方法适用于水面无人艇航迹控制，但在路径曲率改变时，需要较长的时间跟随期望路径。

国内外学者对无人艇航迹控制理论、模型和实现方法等进行了研究，但无人艇实际工作环境复杂多变。鉴于此，本文将根据无人艇实际航行时会受到的导航信号及环境的影响，对无人艇航迹控制进行研究。采用基于模糊控制可变船长比的LOS 算法与自抗扰航向控制器（ADRC）结合的航迹控制方法，首先进行三角形航迹仿真验证，验证算法的可行性。随后进行湖上试验，验证无人艇航迹控制的效果。

1 无人艇航迹控制方法

无人艇在实际航行时，存在风、浪、流、遮挡等环境干扰，会出现偏离目标航线的情况。针对这些问题，需对无人艇航迹控制进行细分研究。本文将无人艇航迹控制分为GPS 信号有效和无效2 种情况，分别介绍航迹控制方法。

1.1 GPS 信号有效时的航迹控制

LOS 算法在水面船舶航迹控制中应用广泛，其实质是控制船舶实际航向对准目标航向的LOS角，如图1 所示。图中，Pk−1点为航迹起点，Pk点为设定目标点，O点为无人艇实时位置，设O点经纬度为（Oj，Ow），Pk−1点经纬度为（P(k−1)j，P(k−1)w），Pk点经纬度为（Pkj，Pkw），PLOS点经纬度为（Pj，Pw）。以无人艇O点为圆心，以可调参数R为可视半径得到一扇形区域。假设船长为L，船长比为n，则R=nL[12]，Δh为航迹偏差。图1 中扇形区域交直线Pk−1Pk于点PLOS，O点与实时目标点PLOS的连线方向为相对于真北方向的实时目标航向Ψr。无人艇实际航向Ψ 由GNSS 设备获取，Ψr和 Ψ 的差为消除航迹偏差的角度ΔΨ 。当无人艇驶入Pk点圆域内时，则切换新的目标点，以此类推，最终完成无人艇的航迹控制。

图1 无人艇LOS 航迹控制原理图Fig. 1 Schematic diagram of LOS track control for USV

1.2 无GPS 信号时的航迹控制

无人艇实际航行时存在过桥洞、船闸等GPS信号无效的情况，为保证航迹，本文采用惯性导航融合处理方法。如图2 所示，以无人艇过桥洞为例，假设无人艇在O点时GPS 信号无效，此时系统需记录最后一次GPS 信号有效时的航速v，惯性导航设备的无人艇艏向 ψX、当前无人艇位置O到目标点Pk+1的距离d。为到达Pk+1点，无人艇以目标航向 ψr=ψX、速度v、持续t=d/v时间。如果GPS 信号持续t时间后仍无效，则此时无人艇目标航向 ψr+1为惯性导航设备提供的无人艇艏向 ψX加 上 下 一 点 的 航 向 偏 差 ∆θ， ∆θ由 式(4)确定：

图2 无人艇有遮挡航迹控制示意图Fig. 2 Schematic of covered track control for USV

式中， ψk+1与 ψk+2分 别为Pk+1点相对于Pk点的目标航向和Pk+2点相对于Pk+1点的目标航向，由式(5)和式(6)求得。依此类推，实现无人艇在GPS 信号无效情况下的航迹控制。

2 无人艇航迹控制算法

对于相同的目标航迹，采用不同的可视半径R具有不同的航迹控制效果，为了能够在不同的环境影响下得到最合适的船长比n，采用模糊控制方法，实时调节n，航向控制采用自抗扰控制器；传统的PID 控制算法参数固定，很难选取合适的控制参数，自抗扰算法具有过渡过程，可实时给予补偿，并可对状态误差进行非线性组合。航迹控制系统结构图如图3 所示，图中，δr为控制舵机的输入量，δ 为作用在无人艇模型上的控制量。

图3 航迹控制系统框图Fig. 3 System diagram of track control

2.1 模糊控制器

图3 中模糊控制器的输入为实时位置距目标航线的航迹偏差∆h和航迹偏差变化率dh，输出为可变船长比n。该系统可根据无人艇环境变化产生的 ∆h和dh实时调节n，从而改变LOS 航迹算法中的R，得出最佳目标航向 ψr。

2.1.1 模糊化与规则库

表1 船长比模糊控制表Table 1 Fuzzy control table of ship length ratio

可变船长比n的作用为：选用较小的n时，当受到风浪流影响，无人艇能够更快地到达目标航线，但容易产生超调；选用较大的n时，无人艇能够平稳地保持目标航向，但受到风浪流干扰，不能快速贴近目标航线。本文模糊规则表的主要思路为：当 ∆h较大时，无人艇偏离目标航线较为严重，应采用较小的n，尽快地将无人艇的位置调节到目标航线附近；当 ∆h较小、dh较大时，说明此时正在受到环境影响，应采用较小的n；当 ∆h较小、dh较小时，说明此时航行比较稳定，应采用较大的n。无人艇实际航行过程中通过 ∆h和dh实时调节可n，可快速适应环境变化。

2.1.2 解模糊

模糊控制器的输出为精确的数值，因此需要进行解模糊运算，解模糊有很多种求法，常见的有加权平均法、中值法、面积重心法等[15]，本文选用面积重心法，具体公式为

式中：k为变量；m为模糊集元素个数；uk为模糊子集元素值；uv(uk)为模糊子集对应元素的隶属度值；∑为输出模糊子集中各元素的隶属度值的求和；u为输出的精确数值。

2.2 自抗扰航向控制器

自抗扰控制器由3 个部分组成：跟踪微分器、扩张状态观测器和非线性状态误差反馈控制律。跟踪微分器安排过渡过程并提取微分信号，解决由于航向突变导致的超调问题；扩张状态观测器不仅对航向值进行观测，还可实现对不确定扰动的估计；非线性状态误差反馈控制律根据过渡过程与状态估计之间误差的非线性组合和扰动估计量的补偿来生成控制信号[3]，自抗扰结构如图4所示。图中，V0(k)为目标航向，V1(k)为V0(k)的过渡过程，V2(k)为过渡过程的微分，e1(k)和e2(k)为过渡过程信号与观测信号的误差函数，U0(k)为中间控制量，U(k)为输入舵角值，Y(k)为实际航向，Z1(k)～Z3(k)为系统状态的观测量，b0为增益参数。

图4 自抗扰控制器结构图Fig. 4 Structure diagram of ADRC

实验中采用的跟踪微分器离散表达式及扩张状态观测器表达式与文献[16]中的表达式相同，非线性状态误差反馈控制律采用非线性状态误差的PD 控制律，如式(8)所示。式(8)中非线性函数fal与文献[16]中的相同，kp，kd， δ0，b0为可调参数。

3 控制系统及软件设计

采用国产芯片龙芯1C 和国产嵌入式实时操作系统RT-Thread 作为无人艇控制的自主可控平台，研制的数据采集控制板卡接口丰富，电源、传感器、电机舵机控制信号、通信模块等均接入到数据采集控制板卡，由数据采集控制板卡解析出时间、速度、方位角、GPS 状态、经纬度、姿态角和障碍物距离等。为完成航迹控制任务，采用RT-Thread 国产嵌入式实时操作系统多线程设计。主要分为传感器数据接收处理线程、航迹算法线程、航向控制线程、电机舵机控制线程和上位机数据交互线程，航迹流程图如图5 所示。系统首先接收各传感器发送的数据，解析并提取有效数据，根据GPS 状态选择航迹控制方法，并将计算得到的脉冲宽度调制(PWM)值传给舵机，舵机接收到信号后执行命令。

4 无人艇航迹控制仿真

4.1 仿真环境

根据设计的航迹控制算法使用Simulink 工具搭建航迹仿真环境，Simulink 航迹仿真框图如图6所示。仿真框图主要分为3 个部分：航迹控制环、航向控制环和更新航迹环。

航迹控制环采用基于模糊控制可变船长比的LOS 算法，输入为当前位置、目标点位置和根据模糊控制可变船长比算法求得的可视半径，输出为目标航向。航向控制环采用自抗扰控制器，航向控制环中仿真的无人艇数学运动模型采用Nomoto 模型[17]，表达式如式(9)所示；舵机模型采用一阶惯性模型，表达式如式(10)所示；无人艇航行中受到风浪对航行的干扰可等效到舵角干扰，通过加入白噪声来模拟风浪流的干扰，表达式如式(11)所示。更新航迹环可根据无人艇当前位置、速度和实际航向角求得下一点位置。仿真算法中无人艇位置均采用经纬度坐标表示。

图5 航迹控制软件框图Fig. 5 Software block diagram of track control

4.2 实验仿真

仿真航迹为三角形，仿真时式(8)中的仿真参数为：kp=0.9，kd=1.2， δ0=0.02，b0=0.3，式(11)中的参数为： ξ=0.1， ω0=0.3， σ=0.2，速度设为定值2 m/s。仿真时3 个目标点的经纬度为：A（119.460 934 101 7°,32.198 867 838 3°） ，B（ 119.461 125 181 7°，32.198 695 635 0°） ，C（ 119.460 876 234 6°,32.198 674 479 7°）。以A点为起点，以正北方向为初始航向，以A点到B点再到C点的方向进行航迹仿真，无人艇航迹仿真图如图7 所示。

由图7 可知，无人艇能够沿着设定的航迹行驶且转弯处过渡自然。转弯角度为钝角更能证明算法的快速适应性，从图中可看出，无人艇在到达目标点附近时进行大幅度转弯，此时航迹偏差与航迹偏差变化率较大，模糊控制器便会根据模糊规则输出合适的船长比，得出可更快到达期望航线的目标航向值，使得实际航线快速贴近目标航线，且无较大超调。

图6 Simulink 航迹仿真框图Fig. 6 System diagram of track control using Simulink

图7 航迹仿真图Fig. 7 Diagram of track

图8 为无人艇航向与舵角仿真图。无人艇以正北方向为初始航向，在5 s 内达到目标航向，并在有环境干扰的情况下保持航向稳定且航向曲线平滑，转弯后能够快速达到目标航向且基本无超调现象。无人艇实际航向为0°～360°，在90 s 的时刻无人艇目标航向由264°变为16°时，无人艇先从264°增加到360°之后再从0°增加到16°，满足无人艇实际航行的要求。图8 中舵角值0°表示初始时的中间位置，大于0°表示向右打舵，小于0°表示向左打舵。无人艇在切换目标点时，会先打出比较大的舵角，当达到目标航向时，无人艇舵角逐渐趋于0°，这也证明了本文自抗扰航向控制器的有效性。

图8 无人艇航向与舵角仿真结果Fig. 8 Simulation result of headings and rudder angle of USV

5 试验结果及分析

试验无人艇为双桨双舵，艇长1.4 m，艇宽0.35 m，外接高精度差分GPS 设备的定位精度可达10 cm，组合惯性导航经滤波融合算法后输出航向角度误差达0.5°。采用2 组定制锂电池，每组电池容量为13 600 mA 时，最大负载电流80 A。试验地点为山东日照万平口泻湖，试验天气条件为：气温18 ℃，东北风4 级，路径全程5 km，要求通过4 个桥洞，最窄桥洞宽度为3 m 左右。试验无人艇如图9 所示。在自主航行过程中实时上传无人艇内数据、视频等信息，并在用户界面进行显示，如图10 所示。

图9 试验无人艇Fig. 9 Tested USV

图10 用户界面Fig. 10 User interface

无人艇航行过程中GPS 位置数据都有记录，以经度为横坐标，纬度为纵坐标，将无人艇实际航行5 km 的目标航线和实际航线进行对比，试验中目标航路点有92 个，实际GPS 点有5 000 个。途中穿过4 个桥洞，如图11 所示。

图11 航迹跟踪结果Fig. 11 Track tracing results

由图11 可知，无人艇能够根据设定的目标航线完成自主航行任务，并在无GPS 信号的情况下穿过桥洞。为验证航迹保持精度，对5 km 的航迹点的航迹偏差进行计算，结果如图12 所示，航迹误差均值约为0.1 m，方差约为0.03，最大航迹偏差在1.5 m 左右，是由于此时正在切换航迹点，但无人艇能根据基于模糊控制可变船长比的LOS算法和自抗扰控制器较快地将航迹偏差降到1 m以内。文献[13]“海鲟03 号”无人艇在青岛至连云港之间航行时最大误差在20 m 左右。由此可见，本文给出的基于自主可控平台的水面无人艇航迹控制算法控制精度高，可根据环境情况实时调整，能够满足无人艇实际航行的要求。

图12 无人艇航迹误差Fig. 12 Track error of USV

6 结 论

考虑无人艇航行时受环境的干扰，针对真实环境下无人艇航迹控制问题，设计并实现了基于模糊控制可变船长比的LOS 算法与ADRC 相结合的航迹控制方法，进行三角形航迹仿真实验。仿真结果表明，本文算法能够实现期望路径的航迹控制，经过大幅度转弯过后航迹能够很快恢复到期望航迹，航向稳定且无较大超调，验证了本文算法的稳定性和可靠性。

基于自主可控平台实现了本文给出的控制策略和算法，利用双桨双舵无人艇进行实航验证。试验结果表明，本文策略和方法能够很好地完成真实环境下无人艇的航迹控制，试验航迹误差均值约为0.1 m，方差约为0.03。