[发明专利]一种用于浅航器的垂直入坞停泊算法

说明书

本发明涉及浅航器的自动控制领域，尤其涉及一种用于浅航器的垂直入坞停泊算法，包括以下步骤：设定浅航器机体的期望俯仰角、期望航向角、期望运行速度；根据设定值计算各推进器的实时推进量，控制浅航器运行；浅航器运行至水面，结束控制；优势在于：将运行在水下的浅航器通过水下闭环机动动作，实现在水面的竖直平动行进，以完成入坞停靠动作；为提高浅航器的工况适应能力、提高浅航器的运行便捷性和实用性打下了基础。

技术领域

本发明涉及浅航器的自动控制领域，尤其涉及一种用于浅航器的垂直入坞停泊算法。

背景技术

现有技术中，浅航器自动入坞停泊的方式为水平入坞，然而在一些工况下，浅航器垂直入坞的方式与水平入坞方式相比，会有更高的便捷性和实用性。但目前浅航器的自动控制算法中，并未有用于浅航器的垂直入坞停泊算法。

基于此，本案由此提出。

发明内容

本发明的目的在于提供一种用于浅航器的垂直入坞停泊算法，将运行在水下的浅航器通过水下闭环机动动作，实现在水面的竖直平动行进，以完成入坞停靠动作。

为了实现上述目的，本发明的技术方案如下：

一种用于浅航器的垂直入坞停泊算法，浅航器具备六个正反推进功能的水下推进器，其中两个竖向推进器的推进量表示为T_S1和T_S2，两个横向推进器的推进量表示为T_H2和T_H2，左侧主推进器的推进量表示为T_Z，右侧主推进器的推进量表示为T_Y；

构建浅航器机体坐标系，以浅航器机体长度方向为X轴，宽度方向为Y轴，高度方向为Z轴，浅航器机体绕Y轴转动的角度为俯仰角φ_J，绕Z轴转动的角度为航向角ψ_J；

包括以下步骤：

设定浅航器机体的期望俯仰角φ_r、期望航向角ψ_r、期望运行速度v_r；

根据设定值计算各推进器的实时推进量，控制浅航器运行；

浅航器运行至水面，结束控制；

各推进器的推进量计算公式如下：

其中，L_φ为任意一个竖向推进器到浅航器中心的距离，L_ψ为任意一个横向推进器到浅航器中心的距离，I_φ为绕机体Y轴的转动惯量，I_ψ为绕机体Z轴的转动惯量，k₁和k₂为控制参数，表示浅航器的下降速度，表示浅航器的水平行进速度，tanh为双曲正切饱和函数，表示浅航器机体俯仰角的角速度，表示浅航器机体航向角的角速度。

进一步的，浅航器的期望值设定包括以下内容：

将浅航器的垂直入坞停泊过程分为机体俯仰下潜阶段、机体上仰阶段及机体竖直平动阶段，将三个阶段的期望航向角设为0°，期望速度设为浅航器能够实现范围的任意值；在机体俯仰下潜阶段，将期望俯仰角设定为-45°，在机体上仰阶段，将期望俯仰角设定为0°，在机体竖直平动阶段，将期望俯仰角设定为90°。

本发明的优点在于：通过控制算法，实现浅航器的自动垂直入坞停泊，为提高浅航器的工况适应能力、提高浅航器的运行便捷性和实用性打下了基础。

附图说明