0 引　言

潜艇因故障不能上浮导致艇员被困水下的事故是各国潜艇舰队面临的一个共同风险。据不完全统计，自20世纪20年代以来，世界50多个潜艇国家因非战斗海损造成的失事潜艇数量在170艘以上^[1]。1969年美国核潜艇“长尾鲨”号失事，造成129人丧生，引起了美国军方对援潜救生技术的高度重视^[2]。特别是俄罗斯海军第二代战略核潜艇“库尔斯克”号失事，更引起了各国对援潜救生的普遍重视^[3]。

经过多年发展，美国和欧洲军事强国逐步形成了完整的援潜救生体系。目前处于领先地位的当属美国的“新型潜艇救援潜水和再压缩系统^[4]”和欧洲的“北约新型援潜救援系统（NSRS）^[5]”。近年来，我国在援潜救生装备领域也加大了投入，目前主要以多型救生钟^[6]和LR7型深潜救生艇^[7-8]为主要援潜救生装备。

深潜救生艇从支持母船布放后要航行至失事潜艇位置，进行对接并转移艇员至救生艇内，再航行回母船将艇员安全转移至水面。为了保证深潜救生艇航行性能，提高对接效率和成功率，自动航行控制系统是必要的研究内容。文献[9]对深潜救生艇操纵训练模拟器的控制系统进行研究，但模拟器只能为学员提供操作训练，无法应用于实艇。李彩风等^[10]利用虚拟样机技术对深潜救生装置进行了运动学建模和仿真。何斌等^[11]将S面控制算法和积分分离PID 控制算法应用于深潜救生艇的位置与姿态控制中，取得了较好的效果。张晓棠^[12]建立了简化的四自由度深潜救生艇数学模型，利用四阶龙格-库塔求解出该数学模型的位置和首向角，并对位置和首向角控制效果进行仿真。孙玉山等^[13]采用基于声光引导的参数自适应控制方法，控制深潜救生艇的对接过程，并采用仿真和水池试验的方法进行验证。

针对深潜救生艇航行控制需求，对航行控制系统进行研究，实现深潜救生艇的手动/自动航行控制，并经过海上试验验证了系统的功能。

1 总体结构设计

深潜救生艇航行控制系统的总体结构如图1所示，由主驾控台、从驾控台、主推控制单元、辅推控制单元、状态监控软件、参数监控软件和仿真软件组成。其核心是主推控制单元，主推控制单元根据主从驾控台切换输入的操控指令，进行手动控制、自动定深、自动定向和自动驾驶等运动模式的切换，通过外部传感器的输入数据反馈进行控制信号的解算，输出2台电动主推进器和4台液压辅推进器及主推枢轴阀组的控制指令。主推控制单元主要功能是实现救生艇搜索失事潜艇和对接时的手动和自动操控功能，主要针对主推控制单元的研究和实现进行详细阐述。

2 运动建模 2.1 坐标系及坐标变换

建立深潜救生艇规范、实用的运动数学模型是进行良好控制的前提。根据国际拖曳水池会议（ITTC）推荐的和造船与轮机工程学会（SNAME）术语公报的体系^[14]，采用2种遵守右手螺旋法则的坐标系：大地坐标系 $ E - \xi \eta \zeta $ （简称“定系”），固定于地球；另一个是运动坐标系 $ O - xyz $ （简称“动系”），固联于深潜救生艇，随艇体一起运动。

大地坐标系原点E可选择海中任一点。地理正北为 $ E\xi $ 轴正向，地理正东为 $ E\eta $ 轴正向， $ E\zeta $ 轴正向指向地心，即深度增加的方向。运动坐标系原点 $ O $ 一般选择与潜水器重心重合，沿着潜水器的纵轴方向指向首向为 $ Ox $ 正向，垂直于 $ Ox $ 轴指向右弦方向为 $ Oy $ 轴正向，垂直于 $ Oxy $ 平面指向下为 $ Oz $ 轴正向。

由于运动坐标系不是惯性坐标系，因此分析深潜救生艇的运动时，应先在大地坐标系中建立运动方程，然后转换到运动坐标系中。运动坐标系到大地坐标系的坐标转换矩阵为^[15]：

$ {T_v} = \left( \begin{gathered} \cos \varphi \cos \theta \cos \varphi \sin \phi \sin \theta -\\ \cos \phi \sin \varphi \cos \varphi \cos \phi \sin \theta + \sin \varphi \sin \phi \hfill \\ \sin \varphi \cos \theta \cos \varphi \cos \phi +\\ \sin \varphi \sin \phi \sin \theta \cos \phi \sin \varphi \sin \theta - \cos \varphi \sin \phi \hfill \\ - \sin \theta \cos \theta \sin \phi \cos \theta \cos \phi \hfill \\ \end{gathered} \right) 。$

(1)

根据右手法则， $ \phi $ （横倾角）右倾为正， $ \theta $ （纵倾角）抬首为正， $ \varphi $ （首向角）右偏为正。因为运动坐标系和大地坐标系都是正交坐标系，所以其相互变换的转换矩阵也是正交矩阵，所以：

$ {T_v}^{ - 1} = {T_v}^{\rm{T}}。$

(2)

2.2 空间运动方程

深潜救生艇在水下航行，根据六自由度空间运动，可建立其六自由度空间运动方程^[16]：

$ \left\{\begin{array}{l} X=m[\dot{u}-v r+w q-x_{G}\left(q^{2}+r^{2}\right)+\\y_{G}(p q-\dot{r})+z_{G}(p r+\dot{q})] ，\\ Y=m[\dot{v}-w p+u r-y_{G}\left(r^{2}+p^{2}\right)+\\ z_{G}(q r-\dot{p})+x_{G}(q p+\dot{p})] ，\\ Z=m[\dot{w}-u q+v p-z_{G}\left(p^{2}+q^{2}\right)+\\ x_{G}(r p-\dot{q})+y_{G}(p p+\dot{p})] ，\\ K=I_{x} \dot{p}+\left(I_{z}-I_{y}\right) q r+\\ m\left[y_{G}(\dot{w}+v p-u q)-z_{G}(\dot{v}+u r-w p)\right] ，\\ M=I_{y} \dot{q}+\left(I_{x}-I_{z}\right) r p+\\ m\left[z_{G}(\dot{u}+w q-w)-x_{G}(\dot{w}+v p-u q)\right] ，\\ N=I_{z} \dot{r}+\left(I_{y}-I_{x}\right) p q+\\ m[x_{G}(\dot{v}+u r-w p)-y_{G}(\dot{u}+w q-w)]。\end{array}\right. $

(3)

其中： $ u,\;v,\;w $ 为 $ x,\;y,\;z $ 三轴速度； $ p,\;q,\;r $ 为 $ x,\;y,\;z $ 三轴角速度。

如果运动坐标系选定深潜救生艇的重心，且在控制时不考虑横倾和纵倾运动，忽略水平面和垂直面的运动耦合，则可建立深潜救生艇四自由度运动方程，并得到简化方程如下：

$ \begin{aligned}[b] & X = m(\dot u - vr)，\\ & Y = m(\dot v + ur) ，\\ & Z = m\dot w ，\\ & N = {I_z}\dot r 。\end{aligned} $

(4)

通过此四自由运动方程，可实现救生艇的深度和艏向的闭环控制。

3 主控单元设计 3.1 硬件设计

图3为了主推控制单元硬件电路的结构图。主推控制单元硬件电路以STM32F407ZGT6单片机为核心，采用双电源并联输入，提高电源可靠性。数字量的输入输出均通过磁隔离芯片进行隔离，模拟量输入通过滤波及运放调理电路处理后，采用多个SPI总线式ADC芯片采集，多个ADC芯片挂在一个SPI总线上，并通过总线隔离后接入单片的SPI2总线接口；模拟量输出与模拟量输入模式相同，ADC芯片更换为DAC芯片。主推控制单元通过串口通信电路接收深度计、高度计、DVL和罗经等设备的数据，并与上位机软件通过串口传输数据。每一路串口通信均进行串口隔离，确保电路不受外界接口和线路的干扰影响。

3.2 软件设计 3.2.1 软件结构

深潜救生艇主推控制单元软件实现操控指令采集、传感器数据通信、状态显示控制、手动航行控制、自动定向航行控制、自动定深航行控制、自动驾驶航行控制、数据自动上传等功能。根据软件功能可分为8个模块，如图4所示。

3.2.2 软件流程

软件启动后，首先进行系统初始化，对系统时钟、定时器、IO端口、SPI总线、看门狗、串口通信、中断和控制参数等进行初始化。初始化完成后进入主循环，检测驾控台的开关信号、模拟量信号、编码器脉冲信号、传感器模拟信号等数据，并进行运算处理，生成系统可用的数据。为了提高响应的实时性，外部开关信号和编码器脉冲信号通过外部中断模式进行检测，确保实时性和可靠性。定时器在时钟开启后开始工作，在设定的定时周期产生后将控制信息、传感器数据和控制参数等信息打包上传到上位机参数显示软件进行显示和保存，同时定时进行看门狗喂狗操作，防止程序复位重启。串口通信模块根据数据通信产生串口接收中断，接收传感器串口数据，并进行解析。在主循环中根据驾控台操控指令，选择进入手动控制模块还是自动定向、自动定深或自动驾驶控制模块。自动定向、自动定深或自动驾驶的设定值通过调节开关可以调整设置，通过外部中断对调节开关的脉冲信号进行检测，根据调节开关的调节方向进行设定值的增减。图5为主推控制单元软件流程图。

3.2.3 自动控制算法实现

深潜救生艇自动控制功能包括自动定向和自动定深功能，分别通过控制侧推和垂推实现。自由度互相解耦，且功能相似，以自动定向为例，详细介绍自动控制算法的实现。

自动定向功能是根据罗经反馈的首向角，通过对首尾侧推的推力分配控制，实现深潜救生艇保持设定的首向角航行。采用增量式PID控制算法，根据首向角的误差和误差变化率，构成首向角闭环控制。图6为自动定向闭环控制原理图。

深潜救生艇的首向角数值在0～360°范围内，为了趋近设定的首向角，转向运动可以选择左转或右转。但为了节省能源和提高响应速度，应选择最小路径进行转向。根据最小路径原则，转向应选择差值的绝对值在180°以内的路径进行转向。按照此转向原则，在进行误差计算时，将误差值换算成−180°～180°范围内，则误差为正值时右转，负值时左转。具体计算方法分4种情况（以下首向设定值为D_Set，当前首向角为D_Now，首向角误差为Error）：

1） D_Set<=180，D_Now>=0且D_Now<= D_Set+180，Error=D_Set-D_Now；

2） D_Set<=180，D_Now> D_Set+180，Error=D_Set-D_Now+360；

3） D_Set>180，D_Now>=0且D_Now< = D_Set-180，Error=D_Set-D_Now-360；

4） D_Set>180，D_Now>D_Set-180，Error=D_Set-D_Now。

4种情况下Error值都被换算到–180°～180°范围内，当Error>0时，执行右转，Error<0时，执行左转。在实际程序处理时，为了防止推进器频繁启动，根据自动定向精度要求，设置了±0.5°的死区，误差在死区内，认为定向目标已达到，推进器不动作，主要是避免推进器频繁的启动。控制推进器转速的比例阀控制量则由PID算法解算得到。自动定向模块软件流程图如图7所示。

4 试验验证

航行控制系统搭载深潜救生艇完成了海上试验。海试期间，深潜救生艇搭载海军某舰在青岛海域开展了与模拟试验平台（模拟潜艇出入舱口）的对接试验。试验开始前，将模拟平台吊放入海底，然后母船驶离模拟平台，距离1 km左右。然后将深潜救生艇从船尾吊放入水，操作员通过声学定位装置搜索模拟平台的位置，采用自动定向、自动定深模式，操作深潜救生艇向模拟平台航行。到达模拟平台上方时，不断调整救生艇的深度和姿态，最终实现救生艇的对接裙口与模拟平台的模拟潜艇出入舱口对接。对接后通过海水泵将对接裙内海水排出，建立空气环境，打开对接口顶部舱口盖。将数据保存在文件中，试验结束后进行分析。

4.1 自动定向

航行控制系统搭载深潜救生艇完成了海上试验，在海试期间进行了多次自动定向测试，这里选取其中一次自动定向的数据进行分析。图9为自动定向控制曲线。从16:22:35开始进入自动定向控制模式，设定首向角为22.8°，到16:28:50退出自动定向模式，持续6 min 15 s时间，救生艇航速逐渐增大，最大航速为0.972 m/s，首向角最小值为22.1°，最大值为23.7°，误差均在1°以内，控制效果良好。

4.2 自动定深

图10为自动定深控制曲线。从16:57:34开始进入自动定深控制模式，设定深度为13.9 m，到17:16:55退出自动定深控制模式，持续19 min 21 s时间，期间救生艇航速时快时慢，最大航速为2.14 m/s，救生艇深度在13.7～14.1 m之间变化，深度误差保持在0.2 m以内，控制效果良好。

5 结　语

针对深潜救生艇航行控制需求，建立了深潜救生艇航行控制硬件体系结构。通过坐标系的转换，对水平面和垂直面运动方程进行简化，得到了深潜救生艇的运动数学建模。以STM32单片机为核心处理器，进行主推控制单元的软硬件系统设计，通过双电源冗余备份设计、采样调理和隔离等措施，提高系统在强干扰环境下的安全可靠工作。通过半物理仿真验证，提高了系统研制的效率，有效降低了实艇测试的风险。通过对海试过程中自动定向和自动定深数据的分析，表明自动航行控制效果良好，定向控制精度在1°以内，定深控制精度在0.2 m以内，航行控制系统性能稳定可靠，为深潜救生艇全面国产化奠定了坚实基础。