0 引言

在发展海洋经济的同时, 各种各样的海洋观测、开发工具不断涌现.其中USV(unmanned surface vehicle)以其成本低廉、隐蔽性好、体积小、机动灵活、无人员伤亡危险等优点, 在水质采样、水文测绘、气象探测、海底探测、通讯中继、人员搜救、海岸巡逻、反潜扫雷等方面^[1-3]具有广阔的应用前景.在USV的应用中, 精确的模型对实现USV的自主运行起着非常重要的作用.它是系统能够稳健运行的重要保证.然而, 由于水面上存在大量的风、浪与水流扰动, 并且水动力学机理异常复杂, 建立高精确的模型存在一定的困难^[4].

USV的数学模型主要有线性模型与非线性模型两类.线性模型典型的代表是1957年Nomoto提出的1阶Nomoto模型^[5], 它已被广泛使用并成为USV的主流建模方法^[6].通常Nomoto模型在2个假设下是有效的：1)USV的速度随时间不变或者变化非常缓慢(速度损失可以忽略不计)^[7]；2)USV的惯性足够大, 以确保USV处于排水区^[8].然而, Nomoto模型的不足之处也很明显：首先, 描述USV非线性动力学^[9]和侧滑动力学^[10]时表现不佳；其次, Nomoto模型的参数随USV前向速度的变化而变化, 降低了控制性能.非线性模型的典型代表是1964年Abkowitz提出的Abkowitz模型^[11]. Abkowitz模型的主要思想是采用非线性3阶截断泰勒级数展开式来逼近名义条件下的流体动力学.为了辨识名义条件下的流体动力学导数, 需要精确测量施加在USV上的合外力与合外力矩.然而, 在工程上, 通常不具备精确测量的实验条件^[12].

在得到USV的实际模型之后, 本文要解决的核心问题是如何设计一个控制器, 将外在扰动融入到控制器设计中, 以实现稳定、高品质的控制, 目前常用的扰动补偿策略为被动扰动补偿, 而很少考虑扰动补偿机制^[13], 如文[14]和文[15]分别通过H_∞控制和滑模变结构控制来抑制不确定因素与外界干扰对USV航向控制的影响.其中滑模变结构控制通过设计变结构的切换超平面进行反馈, 虽然具有响应速度快、能够克服系统的不确定性的优点, 但容易激发系统未建模动态、诱发系统抖振.文[16]对反转设计法进行了改进, 通过假定外界扰动是有界的, 在控制器设计中考虑了期望轨迹和实际轨迹之间的差异, 保证了系统的稳定性.文[17]通过额外的积分项来减轻干扰作用的影响, 虽然通常能够保证系统对扰动的稳定性, 但增加了系统的超调量.

针对被动扰动补偿策略的不足, 本文提出了一种USV的主动增强LPV(linear parameter varying)建模框架, 该框架分为2个部分：LPV结构化模型与非结构化模型差, 如式(1)所示：

(1)

其中, x是系统的状态、u是系统的输入；A(·)是结构化模型函数, B(·)是系统输出函数, 它们是变参数θ的函数, 并通过变参数θ来反映系统的主要动力学特性；Δx是模型误差, 包括未建模误差与外部干扰.针对模型(1), 可以使用在线估计与辨识算法进行求解, 如EKF(extended Kalman filter)^[18]与ε支持向量回归^[19].由于USV具有强非线性的特性, 本文采用本质非线性滤波器UKF(unscented Kalman filter)进行在线估计^[20].

在主动增强LPV建模框架的基础上, 本文采用LMI(linear matrix inequality)设计了主动增强LPV航向保持控制器.该控制器在未建模误差的估计的基础上, 利用前馈策略进行在线补偿, 从而达到了在线抑制风、浪与水流扰动的目的.上述框架的优点是将名义LPV控制与在线模型差补偿控制相结合, 利用在线计算降低离线参考模型结构简化带来的名义控制性能下降问题.同时, 相对于分段控制, LPV控制方法考虑到了变参数θ切换给系统带来的影响, 提高了控制性能.

1 LPV动力学建模

对于USV而言, 其Froude数定义为^[8]

(2)

式(2)中, u为USV的前向速度, g为重力加速度, l为USV的浸水长度.

当Froude数小于0.5时, USV受排出水的静压力作用主导, 处于排水区；当Froude数大于0.5且小于1时, USV同时受排出水的静压力与滑行时的动压力, 处于半排水区；当Froude数大于1时, USV受滑行时的动压力作用主导, 处于滑水区.由于USV从排水区到滑水区, 动力学特性具有明显差异, 本文提出将USV的前向速度(正比于Froude数)作为线性变参数, 用来简化USV的动力学建模.

1.1 坐标系定义

一般而言, USV的横摇、纵摇与垂荡运动可以忽略^[21], 因此USV在水面上的运动可以近似看成是一个平面运动.如图 1所示, 建立USV的TP(tangent plane)坐标系与本体坐标系, 其中, X_E指向正北, Y_E指向正东；X_B指向USV的正前方, Y_B指向USV的正右方；ψ为USV的航向角；u、v、r分别为USV在本体坐标系中的前向速度、侧滑速度、转艏角速度.

1.2 非线性动力学建模

对于USV的刚体动力学, 其动力学特性受USV的质量m、USV绕质心(x_G, y_G)的转动惯量I_zz决定且有动力学方程：

(3)

其中, 模型输入为方向舵量δ与油门百分比ε；系统状态为前向速度u、侧滑速度v、转艏角速度r；剩余参数均为常数, 由USV的质量m、转动惯量I_zz、质心的坐标(x_G, y_G)及水动力学导数决定, 需要进行离线辨识求解.

1.3 非线性动力学LPV简化

虽然非线性模型具有较高的建模精度, 但在实际系统中, 二次非线性项容易放大风、浪与水流的扰动, 不利于辨识精确的模型.因此, 需要对系统进行适当的简化：一方面反映系统的主要动力学特性, 另一方面又方便进行系统辨识与控制.考虑到USV的水动力学特性主要受Froude数(USV的前向速度)影响且舵角较小, 可将非线性模型(3)分解为速度动力学模型与操纵动力学模型.

1) 速度动力学模型：

(4)

其中, , b₁、k₁、k₂为常数.

2) 操纵动力学模型：

(5)

式(5)即为关于前向速度u(正比于Froude数F_r)调度的LPV模型.

将式(4)、式(5)联立写成状态方程的形式有：

(6)

其中,

1.4 模型参数辨识

对于速度动力学(4), 将其离散化后有：

(7)

其中, T_s为采样时间, u[k]为第k个采样周期的前向速度, η[k]为第k个采样周期前向速度的观测噪声, ε[k]为第k个采样周期的油门开量, a₁₁、a₁₂、b₁k₁、b₁k₂为待辨识的参数.

定义：

从而有：

(8)

若采样点的个数n＞5, 则待求解参数θ_u中的变量个数小于方程组的个数, 故上述方程组为过度确定方程组.采用最小二乘法可得

(9)

同样, 对于操纵动力学, 有最小二乘估计：

(10)

其中, _v为[T_sa₂₁  T_sa₂₂  T_sb₂/b₁]的最小二乘估计, _r为[T_sa₃₁  T_sa₃₂  T_sb₃/b₁]最小二乘估计, X_v、H_v, X_r、H_r满足：

2 状态与模型差联合估计

为了消除模型差(风、浪与水流)对控制系统的影响, 模型差必须在线实时的估计并反馈给控制器在线修正, 为此在LPV模型的基础上引入了联合估计^[22].

联合估计将状态和模型差同时进行处理, 即通过构建包含状态和模型差的增广状态, 并建立关于增广状态的模型, 继而使用单一的状态估计方法为增广状态进行估计.假设LPV系统(6)的模型差Δx由噪声h驱动, 则式(6)可重写为

(11)

其中, , w为过程噪声且满足, v为测量噪声.

从以上的估计方程形式不难看出, 联合估计可以获得状态和模型差联合分布的最优或次优估计, 而不是状态和模型差各自的最优或次优估计^[22].此外, 由于联合分布仅需采用单个滤波器, 对原模型无需进行较大修改, 结构更加简单, 只需考虑增广状态的估计问题, 因而被广泛地应用于需要同时对状态和模型差进行估计的场合.

假设采样时间T_s很短, 则, 从而可将(11)离散化：

(12)

其中, w_k和v_k均为互不相关的高斯白噪声且有：

(13)

其中, δ_kj为Kronecker-δ函数.状态初始值x₀^e与w_k、v_k彼此相互独立且服从高斯分布.

考虑到LPV模型(6)是一个受到速度变参数调度的非线性模型, 根据文[23], 本文采用UT(unscented transformation, 见图 2)来计算非线性最优高斯滤波中系统状态的后验均值及后验方差P_k|k-1、、, 并有UKF滤波器：

1) 初始状态统计特性为

(14)

2) 选择UT中的Simga点采样策略. Sigma采样策略有对称采样、单形采样、3阶矩采样及高斯分布4阶矩采样.为了保证输出变量z方差P_z的半正定性, Julier提出了对上述基本采样策略进行比例修正采样^[19], 并有采样公式：

(15)

其中, n为系统状态的维数, λ=α²(n+κ)-n, α为比例缩放因子, κ为比例参数.

3) 时间更新.按照第2)步所选择的Simga采样策略, 由和P_k-1来计算Sigma点ξ_{i, k-1}, 通过状态函数f_k-1^e(x_k-1, u)传播为γ_{i, k|k-1}, 由γ_{i, k|k-1}可得一步状态预测及误差方差矩阵P_k|k-1, 计算公式为

(16)

其中, i∈{0, 1, …, 2n+1}, L=2n+1, W_i^m为均值权系数, W_i^c为方差权系数.

4) 量测更新.同理, 利用和P_k|k-1按照第2)步中所选择的策略计算Simga点ξ_{i, k|k-1}, 通过非线性量测函数h_k(x_k)+r_k传播为χ_{i, k|k-1}, 由χ_{i, k|k-1}可得输出预测量及方差和协方差, 即：

(17)

其中i∈{0, 1, …, 2n+1}.

在获得新的量测z_k后, 进行滤波量测更新：

(18)

3 主动增强LPV控制器设计

在第1节USV建模与第2节USV模型差估计的基础上, 本文提出了USV的主动增强控制框架：LPV名义控制器与主动增强补偿控制器, 如图 3所示.

3.1 名义控制器设计

基于模型(4)与模型(5), 对USV的控制可以划分为航向内环控制器与航速外环控制器.其中外环速度控制器采用反馈线性化方法；在得到前向速度u后, 将前向速度u(正比于Froude数)作为外部线性变参, 用来设计变增益LPV航向控制器.采用这种控制框架具有2个优点：

1) 将全状态控制器转化成了航向内环控制器与航速外环控制器, 简化了控制器的设计难度.

2) 航速外环的速度输出u作为外部线性变参反馈给内环航向控制器, 提高了内环航向控制沿着变参轨迹u的控制性能.

3.1.1 速度名义控制器设计

对于给定的参考速度u_r, 将式(5)写成式(19)的误差形式：

(19)

其中, e_u=u-u_r, w_u为过程扰动.

速度名义控制器的控制目的是使e_u从初始状态渐近稳定到0, 本文选取：

(20)

来抵消非线性项-a₁₂|u|u, 从而得到线性系统：

(21)

因此对于非线性系统(19)的镇定问题转化为对线性系统(21)的镇定问题.进一步地, 可以设计式(22)的状态反馈控制器：

(22)

使得闭环控制系统：

(23)

渐近稳定.最终可得系统的控制率为

(24)

考虑到, 可得使系统稳定的油门输入为

(25)

3.1.2 航向保持名义控制器设计

将航向角ψ增广到操纵动力学模型(5)中, 可得增广的操纵动力学模型：

(26)

其中, sinδ是系统输入, 状态方程中的参数：

w_s=[w_v  w_r  0]^T为过程噪声.

将式(26)写成关于平衡点[v_r, r_r, ψ_r]^T=[0, 0, ψ_r]^T的误差形式, 有：

(27)

因此, 本文的最终目标是寻找一个状态反馈LPV控制器：

(28)

使得闭环系统：

(29)

渐近稳定γ, 其中K(u)为状态反馈控制率.

根据介实引理, 如果系统存在一个正定对称变参依赖的李亚普诺夫函数矩阵P(u)=P₀+uP₁与反馈增益矩阵K(u)=K₀+uK₁, 使得仿射线性矩不等式：

(30)

在4个顶点u∈[u_min, u_max], ∈[-a_max, a_max]成立, 则系统(29)渐近稳定.其中,

得到P(u)、K(u)后, 便可以得到LPV航向保持控制器：

(31)

将式(31)代入到式(28)可得舵量输入：

(32)

3.2 补偿控制器设计 3.2.1 速度补偿控制器设计

在第2节状态与模型差联合估计的基础上, 本文设计了速度补偿控制器.根据速度动力学(19), 为了补偿扰动对名义控制器的影响, 新的速度控制器应满足：

(33)

求解上述逆动力学方程, 可得扰动补偿控制器：

(34)

与使用状态测量反馈的控制器相比, 上述控制器具有2个优点：

1) 使用UKF在线估计的速度反馈给控制器, 能有效地减少传感器噪声给控制器带来的影响, 这是因为UKF估计的状态更接近于系统过程方程中的状态.

2) 使用模型差进行在线前馈补偿, 可以有效地减少模型中的不确定性给系统控制带来的影响.

3.2.2 航向保持补偿控制器设计

与速度补偿控制器不同, 航向保持控制器无法通过系统输入完全补偿系统扰动对控制器的影响.这是由于航向控制模型中存在侧滑速度扰动与转艏角速度扰动, 而实际的航向输入量只有一个.因此需要通过优化的方法进行求解.

与速度补偿控制器设计类似, 新的航向控制器应满足：

(35)

整理可得：

(36)

其中,

显然, 通常情况下, 式(36)不存在 sinδ的解, 因此设计指标函数：

(37)

其中, h₁、h₂为侧滑速度与航向速度的权重.

令, 可得

(38)

从而可得补偿控制器：

(39)

4 仿真

为了验证控制器的有效性, 首先对USV进行了实船参数辨识, 并将辨识结果与Nomoto模型^[5]及线性化模型^[6]进行了对比, 用以验证LPV结构化建模的有效性.然后在实际辨识结果的基础上, 利用状态与模型差进行联合估计, 用以验证主动增强建模的有效性.最后再利用1.2节的非线性模型辨识的参数, 通过搭建USV的动力学仿真平台, 验证主动增强LPV模型相对于常规LPV控制器的优越性.

4.1 实船参数辨识

为了尽可能提高模型参数辨识精度, 本文将模型参数辨识分为2步：直线运动情况下的推力辨识、曲线运动情况下的剩余参数辨识.这是由于直线运动情况下, USV受到的扰动较小, 有利于辨识更为精确的推力模型.

1) 直线运动情况下推力辨识

在这个实验中, USV的舵量输入设定为0, 油门阶跃变化, 用来辨识直线运动情况下的模型参数, 其参数辨识结果如表 1所示, 辨识与验证曲线如图 5所示.

2) 曲线运动情况下剩余参数辨识

在得到直线运动情况下的部分辨识结果后可以使用最小二乘辨识剩余模型参数, 其辨识结果如表 2所示, 辨识与验证曲线如图 6~图 8所示.

采用验证数据得到的前向动力学, 侧滑动力学与转艏动力学误差见表 3.

从表 3的结果可以看出, 简化的LPV模型相对于非线性模型, 其精度略有下降, 但是高于Nomoto模型及线性化模型.此外非线性模型在转艏运动下的辨识结果略低于Nomoto模型及线性化模型.这是由于水面上存在大量的风、浪及水流扰动, 非线性模型中的二次项容易放大这些扰动, 计入到辨识模型中导致模型反而比Nomoto模型及线性模型要差.进一步可以得出, 对于常规USV而言, 前向动力学受扰动的影响较小, 侧滑动力学尤其是转艏动力学容易受到扰动的影响.因此, 需要对建模误差进行实时估计, 尤其是侧滑运动与转艏运动, 这样才能提高USV的航向保持性能.

4.2 模型差在线估计

为了进一步提高USV的建模精度, 本文采用UKF对曲线运动情况下的验证结果进行主动增强, 以验证主动增强的效果.其中, UKF的参数为

(40)

最终得到的滤波误差结果见表 4, 误差曲线见图 9.

从误差结果可以得出, 采用主动增强方法可以大幅提高USV在前向动力学、侧滑动力学与转艏动力学上的建模精度.这是因为, 对于USV而言, 风、浪与水流扰动是时变的, 很难在离线参考模型中精确的考虑.而采用在线估计方法, 可以有效地估计具有低频特性的风、浪与水流扰动.

4.3 主动增强LPV控制器

为了验证不同变参下的控制性能, 在模型辨识与估计的基础上, 本文搭建了USV的仿真平台.在此平台上, 本文进行了阶跃速度跟踪与航向保持跟踪的仿真.根据实际辨识结果, USV的速度满足u∈[1 m/s, 3 m/s].对于变参的加速度, 其范围受USV系统的动力学特性决定, 满足∈[-10m/s², 10 m/s²].在得到变参的速度与加速度后, 就可以得到矩阵不等式(30)关于(u, )的4个顶点(1m/s, -10m/s²)、(1 m/s, 10 m/s²)、(3 m/s, -10 m/s²)、(3m/s, 10 m/s²)的线性矩阵不等式.采用LMI工具可行性问题feasp求解器, 选择γ=0.1可得控制器参数：

将上述参数代入到式(32)可得航向控制器的具体形式, 其仿真结果如图 10所示.

从主动增强的控制结果可以看出, 采用主动增强控制可以有效抑制未建模误差对控制的影响, 提高速度与航向控制精度.相对于常规的鲁棒控制器, 采用主动增强的方法可以在控制器中实时考虑扰动的大小, 而不是最坏情况下的扰动大小, 因而降低了控制器的保守性, 增加了控制性能, 提高了控制精度.此外, 从图 10(b)可以得出, 采用主动增强方法得到的控制器, USV方向舵的输入具有更小的抖动.这是由于采用主动增强方法, 可以有效地消除噪声, 从而得到更加平滑的控制输出.

5 结论

本文针对水面机器人容易受到风、浪与水流干扰的问题, 提出了一种基于LPV模型的主动增强控制方法, 该方法主要解决了2个方面的问题：

1) 建立了水面机器人的LPV模型.实际辨识结果表明所提的LPV模型相对于经典的Nomoto模型有助于提高建模精度.

2) 在LPV模型的基础上, 采用UKF在线估计与前馈补偿策略, 有效抑制了风、浪与水流等干扰对控制器的影响, 提高了航向保持的控制精度.仿真结果证明了控制器的有效性与鲁棒性.