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
名称数值航行器质量m/kg102.04重力加速度g/(m·s-2)9.80航行器转动惯量Jyy/(kg·m-2)14.35连接杆长l/m2.00前翼距航行器重心距离l1/m0.20后翼距航行器重心距离l2/m1.00电机距航行器重心距离l3/m1.90连接杆距航行器重心距离l4/m0.20前翼展长L1/m0.67前翼弦长b1/m0.22后翼展长L2/m0.39后翼弦长b20.13
将无人水翼航行器纵向运动数学模型在工作点:θ=2°、v=6.08 m/s、ξ=-1.04 m处线性化，可得到状态方程为：
(2)
式(2)中：
C为单位矩阵；
为状态变量；
为输入变量，αsi为前后水翼旋转角度；
为干扰变量，ζi为次波面海浪波形。
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
控制方式Δθ均方差/(°)Δξ均方差/mAKF+LQR1.1780.01155KF+LQR1.9080.01248
6 结论
1) 针对无人水翼航行器纵向姿态控制的问题，建立了其纵向运动数学模型并选择LQR控制器作为其纵向运动姿态控制器。
2) 针对传统KF滤波器过程噪声矩阵固定，导致在外界噪声未知情况下滤波精度不足的问题，设计了一种可以根据新息卡方检验值自适应调节过程噪声矩阵的AKF滤波器。
3) 通过仿真对比验证，在基于AKF滤波器的纵向姿态控制器控制下，无人水翼航行器可在随机海浪中平稳运行，有效降低了海浪干扰。
该研究可为无人水翼航行器的控制器设计提供理论依据，也可为其他种类航行器设计提供借鉴。
[1] Liu S,Xu C,Wang Y.Disturbance rejection control for the course keeping of the fully-submerged hydrofoil craft[C]//Proceedings of the 35th Chinese Control Conference,Chengdu,China,2016:747-751.
[2] 王雅璇,罗建南,罗小桃.基于改进人工蜂群算法的主动悬架LQR控制器设计[J].噪声与振动控制,2021，41(03):61-66.
Wang Y X,Luo J N,Luo X T.Design of LQR controllers of active suspensions based on improved artificial bee colony algorithm[J].Noise and Vibration Control,2021,41(03):61-66.
[3] Bai J,Kim Y.Control of the vertical motion of a hydrofoil vessel[J].Ships and Offshore Structures,2010,5(03):189-198.
[4] Rinaldi F,Chiesa S,Quagliotti F.Linear quadratic control for quad rotos UAVs dynamics and formation flight[J].Journal of Intelligent &Computer Sciences,2013,70:203-220.
[5] 李晨,陈永冰,李文魁，等.基于细菌觅食算法的船舶舵减横摇 PID 控制器设计[J].船海工程，2020，49(01):45-47.
Li C,Chen Y B,Li W K,et al.Design of ship rudder reduction roll PID controller based on bacterial foraging algorithm[J].Ship&Ocean Engineering，2020，49(01):45-47.
[6] Kim S H,Yamato H.The estimation of wave elevation and wave disturbance caused by the wave orbital motion of a fully submerged hydrofoil craft[J].Journal of Marine Science and Technology,2005,10:22-31.
[7] Chen H L,Song J H,Chen Y W.The applied research of hydrofoil catamaran attitude estimation based on the fusion filtering[C]//Proceeding of the 34th Chinese Control Conference,Hangzhou,China,2015:28-30.
[8] Gao Z X,Wu D S,Gu H B.Air data estimation based on adaptive Kalman filter[C]//IEEE 4th Advanced Information Technology,Electronic and Automation Control Conference,Hangzhou,China,2019:2516-2520.
[9] 周柳明，王玉龙.船舶舵减横摇鲁棒H∞控制研究[J].控制工程,2016,23(01):102-108.
Zhou L M,Wang Y L.Robust H∞ control of rudder roll damping for ships[J].Control Engineering of China，2016，23(01):102-108.
[10] Chai W,Naess A,Leira B J.Filter models for prediction of stochastic ship roll response[J].Probabilistic Engineering Mechanics,2013,41:104-114.
[11] 胡浩俊,张军,刘志林.考虑附体输入约束的高速多体船预测控制减纵摇方法[J].中国舰船研究，2021，16(04):140-146.
Hu H J,Zhang J,Liu Z L,et al.Predictive anti-pitching control for high-speed multi-hull ship with appendage constraints[J].Chinese Journal of Ship Research,2021,16(04):140-146.
[12] 任俊生.高速水翼船运动控制[M].北京:科学出版社,2015.
Ren J S.High-speed Hydrofoil ship motion control[M].Beijing:Science Press2015.
[13] Zhao L E,Xie Y H.The principle and design of high performance ships[M].Beijing:National Defend Industry Press，2009.
[14] Huang X P,Wang Y.Principle and application of Kalman filter[M].Beijing:Publishing House of Electronics Industry,2015.
[15] 何飞,双丰,许玉云,等.双渐消Kalman滤波补偿六维力传感器矩形板噪声模型误差[J].机械科学与技术,2019,38(04):594-600.
He F,Shuang F,Xu Y Y,et al.Model error compensation of double-fading kalman filtering to rectangular board of six-axis force sensors[J].Mechanical Science and Technology for Aerospace Engineering,2019,38(04):594-600.
[16] Prasad L B,Tyagi B,Gupta H O.optimal control of nonlinear inverted pendulum dynamical system with disturbance input using PID controller &LQR[C]//IEEE International Conference on Control System,Computing and Engineering,Penang,Malaysia,2011:21-27.
[17] 金建海,周则兴,张波,等.无人艇航行仿真关键技术研究[J].系统仿真学报,2021,33(12):2846-2853.
Jin J H,Zhou Z X,Zhang B,et al.Research on USV navigation simulation key technologies[J].Journal of System Simulation,2021,33(12):2846-2853.