基于AKF的无人水翼航行器纵向姿态控制研究

孙佳宇，段富海

(大连理工大学 机械工程学院，辽宁 大连 116024)

1 引言

无人水翼航行器是将水翼与无人航行器相结合，可以大幅提高无人航行器的性能。与机翼类似，水翼工作原理是：在水中运动的水翼依据伯努利原理会产生向上的升力，将航体抬离水面航行，这样就能有效减少水流对航体的阻力和海浪干扰[1]。保证无人水翼航行器的航行姿态，并能在复杂海况中平稳运行是其发挥自身性能的重要前提。本文提出了一种无人水翼航行器，并建立其纵向运动数学模型。LQR控制器性能优良且适用于复杂MIMO系统的控制[2]，因此选择其作为无人水翼航行器纵向姿态控制器。

无人水翼航行器在水中航行过程中需保证姿态稳定，但在实际航行过程中海浪干扰会降低控制器控制性能[3-5]。因此可通过KF滤波器对系统反馈状态进行实时最优估计来改善控制效果[6-8]。传统KF滤波器将过程噪声抽象为方差固定的白噪声，所以在系统和噪声特性已知的情况下，传统KF滤波器可以达到最优估计的效果。但在多数情况下系统的噪声特性均为未知，将导致传统KF滤波器的精度下降,甚至滤波发散。针对海浪滤波，可以利用成型滤波器将海浪有色噪声白化[9-11]，但因为不同海况所对应的成型滤波器不同，所以此类方法并不具有普适性。为解决此类问题，本文提出了一种AKF滤波，可以根据新息卡方检验值自适应调节过程噪声矩阵，以达到自适应滤波的目的。

本文基于一种无人水翼航行器，首先建立其纵向运动数学模型，并设计了纵向姿态LQR控制器；然后针对该控制系统设计了一种基于新息卡方检验值的AKF滤波器,以减小海浪干扰对无人水翼航行器纵向姿态的影响；最后通过仿真验证了基于AKF滤波的纵向姿态控制器,可以使无人水翼航行器在波浪中平稳运行。

2 无人水翼航行器数学模型

无人水翼航行器三维图和结构示意图如图1、图2所示，在达到一定运行速度后无人水翼航行器可进入翼航状态，此时水翼升力可将航体抬离水面。通过伺服电机驱动调整前后水翼的转角，即可改变水翼升力大小，从而可以调整无人水翼航行器的航行姿态，保证其在水中稳定航行。

图1 航行器三维图Fig.1 Three dimensional drawing of the vehicle

图2 航行器结构示意图Fig.2 Schematic diagram of the vehicle

翼航状态下无人水翼航行器纵向受力如图3所示，Fsi为前后水翼(i=1为前翼，i=2为后翼)所产生的力；FDi为水翼所产生的阻力；f为连接杆在水中运动所产生的阻力；F为电机推力。以上参数具体计算方法可见参考文献[12-13]。θ为纵摇角，ξ为无人水翼航行器垂直于水平面升沉量。

图3 翼航状态下航行器纵向受力示意图Fig.3 Longitudinal force diagram of the vehicle under wing navigation

在Zb方向利用牛顿第二定律并以Yb为转轴，利用刚体定轴转动定律，可得到翼航状态下纵向垂荡和纵摇运动数学模型为：

(1)

航行器结构参数如表1所示。

表1 航行器结构参数Table 1 Structural parameters of the vehicle

将无人水翼航行器纵向运动数学模型在工作点:θ=2°、v=6.08 m/s、ξ=-1.04 m处线性化，可得到状态方程为：

(2)

式(2)中：

3 AKF滤波器设计

3.1 KF滤波

KF滤波器通常采用以下离散时间模型来描述：

X(k+1)=ΦX(k)+Gu(k)+Γw(k)

Y(k)=HX(k)+v(k)

(3)

式(3)中：X(k)为系统在k时刻的状态；Y(k)为k时刻对应状态的观测值；u(k)为系统在k时刻的控制量；Φ为状态转移矩阵；G为输入控制矩阵；Г为噪声驱动矩阵；H为观测矩阵；w(k)为过程噪声；v(k)为量测噪声。w(k)和v(k)为不相关的均值为零、方差分别为WQ和WR的白噪声，且系统的初始状态X(0)不相关于w(k)和v(k)，即满足：

(4)

式(4)中，δkj为克罗内克函数。

KF滤波器推导过程可参考文献[14]，具体KF滤波器核心五步式(5)为：

(5)

式(5)中：X(k+1|k)与P(k+1|k)分别为以k时刻为基准的对系统在k+1时刻状态和误差协方差的先验估计值；X(k+1|k+1)与P(k+1|k+1)分别为系统在k+1时刻状态和误差协方差的估计量；Kk(k+1)为系统在k+1时刻的滤波增益。

3.2 AKF滤波

KF滤波器的新息定义为：

e=Y(k+1)-HX(k+1|k)

(6)

在滤波过程中，当状态变量与观测量较为准确时，新息服从标准正态分布。根据KF滤波的正交性原理，新息的马氏距离的平方服从卡方分布[15]，因此可构造基于新息的卡方检验值为：

(7)

式(7)中：φk服从自由度为m的卡方分布，即φk～χ2(m)，m为Y(k)的维数；Ce为新息e的协方差矩阵。Ce计算公式为：

Ce=HP(k+1|k)HT+WR

(8)

KF滤波器将过程噪声抽象为方差不变的白噪声，这就使得当过程噪声变化时，KF滤波器的精度下降，甚至滤波发散。当过程噪声变化时会导致系统的状态发生突变，此时φk将不再服从卡方分布，因此可以根据φk的变化来调整过程噪声矩阵WQ的值，以达到过程噪声矩阵WQ随过程噪声变化而变化的目的。设定φk的上下界来调整一定程度下的过程噪声变化。通过多次仿真实验得知，当φk的上下界分别取置信度为90%和50%对应的卡方值时，滤波效果较好，可得WQ矩阵的判别函数为：

(9)

式(9)中，WQmin和WQmax可根据具体工况进行选取。

4 LQR控制器

LQR算法就是要在对系统可以进行有效控制的前提下付出最小的输入代价[16]，对如下状态空间方程：

(10)

LQR控制的目标就是要找到一个状态反馈增益矩阵K，即系统控制输入为u(t)=Kx(t)，使得如式(11)所示的性能指标达到最小。

(11)

状态反馈增益矩阵K为：

K=-R-1BTP

(12)

矩阵P可通过Riccati方程求得，即：

ATP+PA-PBR-1BTP+Q=0

(13)

式(11)—(13)中：Q矩阵是状态变量的加权矩阵，常取为半正定对角阵，各项系数分别代表设计者对其对应变量误差的重视程度；R矩阵是控制量的加权矩阵，表示能量损失的相对重要性，常直接取单位矩阵。

将AKF与LQR控制器相结合，构成无人水翼航行器纵向姿态控制器，基于AKF的纵向姿态控制系统结构如图4所示。

图4 姿态控制系统结构图Fig.4 Structure diagram of the attitude control system

5 仿真及结果分析

为无人水翼航行器设计LQR控制器，选取控制器参数Q=diag([0;100;0;1 000])，R=eye(2),可得到反馈增益矩阵为：

AKF滤波器参数设为：WQmin=diag(1;1;0.3;0.3])，WQmax=diag([10;10;3;3])，WR=0.003eye(4)。

环境干扰设为有义波高随时间变化的PM波谱随机海浪[17]，有义波高变化如图5所示，遭遇角为120°。

图5 有义波高变化曲线Fig.5 Meaningful wave height variation curve

为验证基于AKF滤波器的纵向姿态控制器的控制效果，以随机海浪为干扰，并与基于KF滤波器的纵向姿态控制器的控制效果作以对比。利用Matlab/Simulink搭建仿真模型，仿真模型框图如图6所示。以升沉量误差Δξ和纵摇角误差Δθ作为评定纵向姿态控制器性能的指标，仿真结果如图7、图8所示。

图6 仿真模型框图Fig.6 Simulation model block diagram

图7 纵摇角误差Δθ变化情况Fig.7 Pitch angle error Δθ Changes

图8 升沉量误差Δξ变化情况Fig.8 Heave error Δξ Changes

不同控制方式数据比较如表2所示，由表2可见，在有义波高随时间变化的随机海浪干扰下，基于AKF滤波器的纵向姿态控制器较基于KF滤波器的纵向姿态控制器控制效果更为优良，无人水翼航行器纵摇角误差Δθ明显变小，升沉量误差Δξ也有所改善。

表2 不同控制方式数据比较Table 2 Data comparison of different control modes

6 结论

1) 针对无人水翼航行器纵向姿态控制的问题，建立了其纵向运动数学模型并选择LQR控制器作为其纵向运动姿态控制器。

2) 针对传统KF滤波器过程噪声矩阵固定，导致在外界噪声未知情况下滤波精度不足的问题，设计了一种可以根据新息卡方检验值自适应调节过程噪声矩阵的AKF滤波器。

3) 通过仿真对比验证，在基于AKF滤波器的纵向姿态控制器控制下，无人水翼航行器可在随机海浪中平稳运行，有效降低了海浪干扰。

该研究可为无人水翼航行器的控制器设计提供理论依据，也可为其他种类航行器设计提供借鉴。