无人艇自适应路径跟踪控制方法设计与验证

2020-07-28文元桥陶威周杰周阳杨吉肖长诗

哈尔滨工程大学学报 2020年4期

文元桥，陶威，周杰，周阳，杨吉，肖长诗

(1.武汉理工大学智能交通系统研究中心，湖北武汉 430063；2.国家水运安全工程技术研究中心，湖北武汉 430063;3.内河航运技术湖北省重点实验室，湖北武汉 430063;4.武汉理工大学能源与动力工程学院，湖北武汉 430063;5.武汉理工大学航运学院，湖北武汉 430063)

近年来无人水面艇(unmanned surface vehicles，USV)作为一种无人水上运载平台在海洋学科领域得到了广泛的研究与应用，如海洋监测[1]、海洋测深[2]、通信中继[3]、桥梁测绘[4]、水质监测[5]和水上搜救[6]等。当USV在执行上述任务时，需要有精确的、可靠的和适应性强的模型、制导方法和控制系统作为支撑[7]。在建模过程中，由于USV的运动受到风和水流以及流体阻力的影响，导致使用传统方法建立USV精确动力学模型时难度较大，并且所建模型在不同的水域和工作环境中缺乏普适性[8]。因此，采用特征建模方法建立更精准的USV模型。特征建模根据被控对象动力学特征、环境特征与控制目标和性能要求相结合,克服只考虑动力学分析建模的不足,建立以利于控制器设计的模型[9]。在艏向和速度控制方法中，PID算法最为经典并较早地应用于实际，但传统PID算法不能很好的适用于复杂多变的环境[10]；滑模算法[11]虽然有较快的响应速度，但其本身对系统模型的精确性有较高的要求；模糊控制算法[12]针对一个控制对象需要众多的模糊系统参数与之对应，整定参数的过程繁琐，容易出现频繁的超调。针对上述不足，结合所建立的特征模型，设计一种调节简单、鲁棒性强的基于全系数自适应控制方法[13]的艏向和速度协同控制器。在路径跟踪控制中的制导律主要是常规的瞄准线(line-of-sight,LOS)制导算法、针对外界环境干扰的积分瞄准线(integral line-of-sight,ILOS)制导算法以及L1制导算法[14-17]。但是多数制导算法只适用于对直线参考路径的跟踪。而实际应用中的USV因航行和作业任务的需要，应能同时完成对直线路径和曲线路径路径跟踪的任务。

本文以武汉理工大学iNav-IV型USV为研究对象针对USV在实际航行中的路径跟踪控制问题进行研究，利用双推进器USV的特征模型，设计一种基于全系数自适应控制算法的自适应艏向和速度协同控制器，并由矢量场(vectro field,VF)制导算法[18]构成路径跟踪控制器外环制导环节。将所设计的自适应路径跟踪控制器部署到iNav-IV型USV上，并在武汉理工大学实验水池进行实船实验。

1 无人水面艇特征模型和参数估计

1.1 USV的特征模型

USV在风浪流及外界干扰下，其运动数学模型是一个非线性高阶方程，考虑对于整体型模型结构下的船舶运动数学模型，常见的处理方法是从整体的角度把作用在船体上的流体动力对每个运动变量按泰勒级数展开，进行线性化处理，忽略高阶小量，简化至2阶或3阶所得的降阶模型。无论是简化后的2阶模型还是3阶模型，都会使原高阶模型里包含的信息有所丢失，而特征模型与高阶系统的降阶模型不同，它是把高阶模型有关信息都压缩到几个特征参量之中，并不丢失信息[19-21]。双推进器型USV从整体模型结构建模时进行线性化处理后可以看作一个多输入多输出线性时变系统，考虑对象动力学特征、环境特征和控制性能要求，本文采用特征模型[9]来代替原有的船舶运动数学模型进行控制器设计。

定理1对于多输入多输出线性时变系统如式，当要实现位置保持或位置跟踪控制时，在采样周期和状态参数满足一定条件下，其特征模型可用输出解耦型二阶差分方程组描述：

X(k+1)=F1×X(k)+F2×X(k-1)+

G0×u(k)+G1×u(k-1)

(1)

其中：

特征模型与对象动力学模型不同，特征模型主要体现的是控制量与要求输出量之间的特征关系，是由特征变量与参量组成的模型。

1.2 USV特征模型的建立

图1为iNav-IV型USV平面运动示意图，ψ为USV艏向角，u和v分别为在USV附体坐标系下船体bx轴和by轴上的速度,f1和f2分别为USV左右电机的推力。

图1 iNav-IV型USV平面运动示意Fig.1 Schematic depiction of iNav-IV USV

双输入双输出USV的特征模型方程表示为：

(2)

其中：

式中:F1、F2、G0和G1为可辨识特征模型参数;x1(k)表示k时刻艏向角;x2(k)表示k时刻速度;u1(k)表示k时刻左推进器电机指令;u2(k)表示k时刻右推进器电机指令。

1.3 在线参数估计

首先，根据从USV上GPS得到输出的经纬度数据计算出USV在NED坐标系下正东方向和正北方向上的线速度分量Ev和Nv[22]。然后，结合电子罗经输出的USV艏向角ψ，可得到在USV附体坐标系下船体bx轴上的速度u和by轴上的速度v：

(3)

将上面得到的USV纵向速度u和艏向角ψ以及左右电机指令u1(k)、u2(k)数据代到式(4)中。尽管是多变量，对于USV特征模型按照每路独立进行在线参数辨识，辨识方法采用带遗忘因子λ的递推最小二乘法[23]的方法。从而在线辨识出F1、F2、G0和G1项的值，即可实现对USV特征模型参数的在线参数估计。

(4)

2 基于特征模型的全系数自适应控制器

全系数自适应控制结构如图2所示，虚线框内的为全系数自适应控制器。

图2 基于特征模型的全系数自适应控制结构Fig.2 The all-coefficient adaptive control based on characteristic model

建立的USVWM是维持跟踪控制器；WH是黄金分割自适应控制器[24]；LI是积分控制器；u(k)是控制量总输入；WD表示被控对象；Y(k)是期望输出与实际输出的误差。二阶特征模型、总控制量由黄金分割自适应控制器、维持跟踪控制器和积分控制器组成：

X(k+1)=F1X(k)+F2X(k-1)+

G0U(k)+G1U(k-1)

(5)

u(k)=uh(k)+uw(k)+ui(k)

(6)

式中：设期望输出为Xr(k)，实际输出为X(k)；参数估计值为F1、F2、G0和G1。

1)黄金分割自适应控制器。

黄金分割自适应控制器是针对构建了特征模型的系统，构成一个黄金分割比例与最小方差相结合的黄金分割自适应控制：

uh(k)=-[G0+λ]-1·[l1F1Y(k)+

l2F2Y(k-1)]

(7)

式中：Y(k)=Xr(k)-X(k)；l1=0.382l2=0.618，F1、F2是特征模型中的待辨识参数，λ为可调参数，其作用是防止矩阵G0奇异。

2)维持跟踪控制器。

控制量uw(k)是为了保持输出X(k)为一定值或跟踪某一理想输出曲线。维持控制量中的可变参数是由估计参数计算所得。控制律：

uw(k)=[Xr(k+1)-F1·Xr(k)-

F2·Xr(k-1)]×[G0]-1

(8)

3)积分控制器。

ui(k)保证系统在参数估计有偏差时有较好的动态品质，各回路采用各自的积分控制有助于调节：

ui(k)=kiY(k)+cui(k-1)

(9)

式中ki、c为可调参数。

3 自适应路径跟踪控制器的设计

如图3所示，USV路径跟踪控制器采用间接控制方法，即制导环节与控制环节分离的结构，由内环基于特征模型的USV艏向角和速度控制器和外环VF制导律2部分组成。基于矢量场制导律根据参考路径信息和USV的运动状态数据输出USV跟踪参考路径所需要的参考艏向角指令ψcmd以及提前设定好的速度值v。VF制导环节如图4所示。

图3 自适应路径跟踪控制器结构Fig.3 Adaptive PF controller structure

图4 VF制导算法Fig.4 VF guidance law for USV

3.1 VF直线制导

VF直线制导算法如式(11)中所示：

(10)

式中:ds为USV到参考直线的垂直距离;χp为船艏向与USV和参考点连线与正北方向的夹角。通过设定χms、ks值可计算出USV从当前船位跟踪参考路径所需要的艏向角度ψcmd(即式中χd),从而实现USV路径跟踪过程中的直线制导环节。

3.2 VF圆形制导

VF圆形制导算法为：

(11)

式中：dc为USV到参考圆的距离误差，χOrbit为参考圆心点USV当前位置点的连线与正北方向夹角的角度。通过设定χmc、kc值可计算出USV从当前船位跟踪参考路径所需要的艏向角度Ψcmd(即式中χd)，从而实现USV路径跟踪过程中的圆形制导环节。

4 实验与结果分析

4.1 iNav-IV型USV系统架构和实船实验环境

iNav-IV型USV是一艘纯电动的双推进型USV，长1.75 m、宽0.75 m、吃水0.12～0.13 m，推进器间距0.4 m。该型USV的推进器为2台直流电机推进器，配置有惯性测量与导航系统、差分GPS系统和船岸无线通信系统。

实验当天风向为东北、最大风速约为3.18 m/s。实船实验中最小二乘算法中的遗忘因子取0.98；全系数自适应算法中，在速度和艏向2个控制回路中ki分别取值1、1，c分别取值-0.2、-0.1；VF制导律中的χms设为90,ks设为0.8，χmc设为90，ks设为0.7。

4.2 艏向和速度协同控制实验及结果分析

为检验USV特征模型和全系数自适应控制器参数在线估计和艏向与速度协同控制的实际效果。设计一组期望速度为1.0 m/s、转艏幅度为100°左右的大角度转艏艏向与速度协同控制实验。

如图5所示，实验结果证明基于特征模型的全系数自适应艏向和速度协同控制器能够较好的实现艏向和速度的协同控制。

图5 船速1.0 m/s的USV艏向控制实验Fig.5 The heading control results of vessel speed at 1.0 m/s

4.3 路径跟踪实验及结果分析

将所设计的USV自适应路径跟踪控制器部署到iNav-IV型USV上。USV在实际执行任务时的参考路径通常是由多端直径路径和曲线路径组成的复杂路径组成。因此，设计以0.8 m/s的航速对如图6所示的复杂路径进行跟踪的实验，包括大角度机动、连续弧形路径等，参考路径包括6段直线路径、3段曲线路径和1个圆形路径。

图6所示的复杂路径跟踪实验的结果表明iNav-IV型USV在各路径段衔接处的航迹误差较大，跟踪复杂路经的航迹误差在±2 m以内，可以较好地完成复杂路径跟踪任务。

图6 复杂路径径跟踪实验结果Fig.6 The results of complex-path guidance

4.4 相对恶劣环境下的仿真验证

实船实验船只为小型测绘无人艇，由于其自身条件限制(最大船速2 m/s)只进行低速条件下的实船实验。ROS系统中的Gazebo仿真软件是一款可以为机器人增加现实世界的物理属性、构建传感器模型甚至可以为传感器添加噪声的物理仿真软件，利用如图7所示的Gazebo软件进行仿真实验，以检验在航速更高，实验环境干扰较大的条件下验证路径跟踪控制器的控制效果。