基于模型预测控制的潜艇水平面航迹控制算法

张子昌，林莉，徐雪峰，赵光

天津航海仪器研究所九江分部，江西九江 332007

0 引 言

潜艇的水平面操纵控制可分为航向控制和航迹控制2种形式［1］，航迹控制指的是被操控对象在操控系统的作用下，从起始位置能沿着预定轨迹航行到目的地，但由于潜艇一般不装备侧向推进器，而只采用方向舵控制潜艇的艏向角，所以潜艇的航迹控制与一般欠驱动船舶以及欠驱动水下潜器的航迹控制相似，都存在欠驱动的特性。

近年来，欠驱动水下潜器以及欠驱动水面船舶的航迹控制问题一直是学术界备受关注的热门课题。李铁山等［2］针对水面船舶的直线航迹控制问题采用了重定义输入输出线性化方法，将单输入多输出误差模型简化为单输入单输出模型，从而简化了控制器的设计，但设定的直线航迹较为简单，具有一定的局限性。Oh等［3-4］运用模型预测控制研究了水面船舶的直线路径跟踪控制问题，并与传统的PID控制效果进行对比，验证了模型预测控制算法的优势。刘昌鑫等［5］针对水下机器人的曲线路径跟踪控制的约束问题，采用模型预测控制得到了能满足约束要求的控制输入，验证了模型预测控制算法对约束问题的求解能力。

但上述研究主要是针对特定的直线或者曲线路径进行分析，而潜艇执行任务时，许多航迹由多个离散航迹点构成。所以，本文将针对某型潜艇的水平面航迹控制问题，首先采用视线导航法（Line-of-Sight，LOS）将航迹控制问题简化成艏向控制问题，艏向控制算法设计上，在增量形式的状态空间模型的基础上设计模型预测控制算法，并将通过由离散航迹点构成的航迹控制仿真实验来验证控制算法的有效性。

1 系统模型建立

1.1 潜艇水平面运动模型

参照目前国际上普遍采用的国际拖曳船模水池会议（International Towing Tank Conference，ITTC）推荐的坐标系，其坐标轴设置如图1所示，其中：E-ξηζ为固定坐标系；O(G)-xyz为运动坐标系。

图1 潜艇的运动坐标系以及固定坐标系Fig.1 Body-fixed frame and earth-fixed frame for submarine

由于潜艇垂直面的运动与水平面运动之间的耦合作用较小，只考虑潜艇的水平面运动情况时可忽略潜艇的垂直面运动项，故结合施生达［6］给出的潜艇标准运动方程，对其进行转换，得到潜艇的水平面动力学以及运动学方程如下：

式中：m为潜艇质量；ρ为潜艇所处深度的海水密度；L为艇长；u，v，r，p分别为潜艇的纵向速度、横向速度、转艏角速度和横倾角速度；分别为纵向加速度、横向加速度、转艏角加速度以及横倾角加速度；Fxp为螺旋桨推力减去阻力在纵向上的分量；Ixx和Izz为潜艇的转动惯量；φ，ψ，ξ，η分别为潜艇的横倾角、艏向角、纵向位移以及横向位移；W，B分别为潜艇静载和潜艇受到的浮力；δr为潜艇方向舵舵角；其它项为潜艇的无因次化水动力系数项。

1.2 LOS

目前学术界研究水面船舶及水下潜器的航迹控制问题主要有2种思路：一种是在Serret-frenet坐标系下直接建立误差模型［4-5，7］；另一种为运用LOS将航迹控制问题简化为更为简单的航速控制、艏向控制以及纵倾控制问题［3，8-9］，针对由离散航迹点构成的航迹，本文采用更为适用且易于实现的LOS。

航迹控制的目标如图2所示。要控制潜艇收敛到由水平面离散航迹点(...,Pk-1,Pk,Pk+1,...)构成的航迹，则艇体质心至航迹段(Pk-1,Pk)的垂直距离（即航迹偏差ek）需收敛至0。

设定航迹点Pk-1的坐标为(xk-1,yk-1)，Pk的坐标为 (xk,yk)，潜艇当前的位置点为P(t)=[x(t),y(t)]，αk-1为航迹段 (Pk-1,Pk)与固定坐标系纵向坐标轴ξ之间的夹角。本文采用Breivik［9］的第 2 种方法，其具体过程可概括为：以当前位置点作一个半径为R1的圆，R1=n1L（n1为正实数），当R1足够大时，所作圆必与航迹点之间的连线相交，将交点中更接近于Pk的交点定为视线导引点Plos(xlos,ylos)，即可求出导引艏向角ψlos作为潜艇的期望动态艏向角：

图2 直线航迹段视线导航法示意图Fig.2 LOS guidance of a straight line

为求解出视线导引点Plos的位置坐标，也即所作圆与直线航迹段的交点，需求解下列方程组：

令 Δx=xk-xk-1，Δy=yk-yk-1，则当 Δx≠0且Δy≠0时，可解得

式中：a=1+d2，其中，其 中g=f-de，e=xk，f=yk；c=x(t)2+y(t)2+g2-2gy(t)-R2。当 Δx=0时，可得

同时，潜艇至航迹段(Pk-1,Pk)的航迹偏差e(t)可由下式计算得出：

当艇体质心位于以航迹点Pk为圆心，半径为R2=n2L（n2为正实数）的圆内时，将导航的目标点切换至下一个目标点Pk+1。通过采用LOS，求取艇体质心至视线导引点Plos的视线导航艏向角ψlos，控制潜艇的艏向角ψ收敛至ψlos，可确保艇体位置收敛至直线航迹段(Pk-1,Pk)，对此Breivik［9］给出了详细的证明过程，在此不再赘述。

综上所述，运用LOS能将水平面的航迹控制问题转化为较为简单的艏向控制问题，也即将问题转换为问题。

2 艏向控制器设计

模型预测控制（Model Predictive Control，MPC）由于采用了多步预测、滚动优化以及反馈校正等控制策略，使其一般具有控制效果好、鲁棒性强等优点，同时考虑到潜艇方向舵的舵角与舵速的约束要求，模型预测控制另一个明显的优势便是可以将各类约束条件加入到控制过程当中，使得控制系统更具有安全性［10］。因此，本文设计了一种基于增量式状态空间模型的预测控制算法用于潜艇的艏向控制。

2.1 线性化模型

由于潜艇水平面运动的非线性模型十分复杂，不宜直接作为控制器设计的模型，故需将潜艇的水平面运动模型简化成线性模型，以便于控制器的设计。本文假定潜艇的纵向速度u保持不变，即=0，且潜艇航行时的深度保持不变，同时忽略垂直面运动与横摇运动对潜艇水平面运动的耦合作用，将潜艇的水平面运动非线性模型线性化成状态空间形式的系统模型，其结果形式如下：

即

再将其转换为离散时间的线性状态空间模型：

式中，I为单位矩阵。

令

即

2.2 未来状态预测

设定预测时域Ny以及控制时域Nc，通过迭代式（12），可得到系统的预测模型

式中：z(k+n|k)为k时刻预测k+n时刻的系统状态值；Δu(k+n|k)为k时刻预测k+n时刻的系统输入增量值；y(k+n|k)为k时刻预测k+n时刻的系统输出值。

2.3 约束优化

将状态空间模型中的输出y重新记为ψ，为简化公式，将输入u简单记为δ，即δr=δ，性能指标函数可定义为：

式中：WQ=diag(q1,…,qNy)，为Ny维输出误差权值矩阵；WR=diag(r1,…,rNc)为Nc维输入权值矩阵。

对于潜艇的水平面航迹跟踪控制问题而言，由于执行机构的物理约束，方向舵的幅值大小与方向舵的舵速大小均受到一定的限制，即控制输入量需满足一定的约束条件，δ(k)需满足

式中：δmin，δmax分别为舵角的饱和下、上限值；Δδmin，Δδmax分别为舵速的饱和下、上限值。因此，预测控制问题便转化为实时的在线约束优化问题：

将式（13）代入式（16）并展开，得到

式中：S=HTWQH+WR；f=HTWQT(Pz(k)-Ψlos)。式（18）即标准的约束二次规划问题，采用一般的二次规划算法便可求出式（18）的最优解。设定由式（18）计算出的最优解为，依据模型预测控制的滚动优化策略，取最优解的第一列即可作为当前时刻的控制器输出增量值，也即当前时刻的期望舵角增量值：

则当前时刻的控制器最优输出值为上一时刻控制器输出值加上当前时刻控制器输出的最优增量值，便得到当前时刻的期望舵角值为

在此基础上进行滚动优化，便可得出整个控制过程的最优控制输入。

3 仿真实验结果及分析

本文采用Matlab/Simulink仿真工具，首先设定水平面离散航迹点，并由潜艇实时位置信息，采用LOS推算出动态的期望艏向角ψlos，再由MPC艏向控制器解算出期望的控制输入量，也即期望的方向舵舵角δr*，并通过简单的执行器闭环得出实际的系统控制输入量δr。整个控制系统的原理框图如图3所示。

图3 航迹控制原理框图Fig.3 The system schematic of track control in horizontal plane

图4 潜艇航迹点控制曲线Fig.4 The curve of submarine's track control

设定方向舵的舵角以及舵速需满足的约束条件为 -15°≤δr≤15°,-1.5°≤Δδr≤1.5°，预测时域Ny=60，控制时域Nc=5，设定的离散航迹点序列为（（0，0），（-4 000，4 000），（0，8 000），（8 000，12 000），（20 000，6 000），（24 000，0），（16 000，-4 000），（8 000，0），（4 000，-4 000），（0，0）），潜艇的初始位置为（0，0），初始艏向角为 90°，航速u=9 kn，艇长L=80 m，n1,n2分别取3和6，航迹控制的仿真效果示意图如图4所示。

由图4可看出，结合LOS与MPC，潜艇能有效实现水平面的航迹控制。在此基础上，分别采用MPC控制方法与传统比例积分微分（Proportion-Integral-Derivative，PID）控制方法进行仿真实验，2种控制方法下的航迹偏差、舵角以及艏向角的仿真结果分别如图5～图7所示。由图5可知，除在航迹点切换阶段航迹控制的偏差较大之外，2种控制方法下的潜艇航迹偏差均保持在一个较小的范围内，符合航迹控制的要求。但是综合分析图6和图7可看出，在航迹偏差大致一致的情况下，MPC控制方法相较传统的PID控制方法能有效降低潜艇的打舵次数，降低了舵机的损耗，同时，控制系统的输出，也即艏向角相比PID控制方法下的输出也更为光滑；并且通过采用MPC，可将舵角以及舵速有效地限制在预定的幅值范围内，验证了预测控制中约束优化的有效性。

图5 航迹偏差曲线Fig.5 The curve of tracking error

图6 控制器输出量曲线Fig.6 The curve of controller output

图7 艏向角曲线Fig.7 The curve of heading angle

4 结 语

本文针对潜艇的水平面航迹控制问题，首先运用LOS简化了潜艇的航迹控制问题，并采用MPC用于潜艇的艏向控制。通过仿真验证，所设计的MPC能有效解决控制过程中的方向舵舵角与舵速的约束限制问题，并且相较传统的PID控制方法能够有效减少打舵次数，证明了预测控制算法所具有的优点。同时，仿真实验也验证了采用结合LOS与MPC的航迹控制方法能够将航迹偏差稳定在一个较小的区间范围内，从而证明了所设计的航迹控制算法能够有效解决潜艇的水平面航迹控制问题。