一种用于浅航器的垂直入坞停泊算法的制作方法

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


背景技术：

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


技术实现要素：

4.本发明的目的在于提供一种用于浅航器的垂直入坞停泊算法，将运行在水下的浅航器通过水下闭环机动动作，实现在水面的竖直平动行进，以完成入坞停靠动作。
5.为了实现上述目的，本发明的技术方案如下：
6.一种用于浅航器的垂直入坞停泊算法，浅航器具备六个正反推进功能的水下推进器，其中两个竖向推进器的推进量表示为t
s1
和t
s2
，两个横向推进器的推进量表示为t
h2
和t
h2
，左侧主推进器的推进量表示为tz，右侧主推进器的推进量表示为ty；
7.构建浅航器机体坐标系，以浅航器机体长度方向为x轴，宽度方向为y轴，高度方向为z轴，浅航器机体绕y轴转动的角度为俯仰角φj，绕z轴转动的角度为航向角ψj；
8.包括以下步骤：
9.设定浅航器机体的期望俯仰角φr、期望航向角ψr、期望运行速度vr；
10.根据设定值计算各推进器的实时推进量，控制浅航器运行；
11.浅航器运行至水面，结束控制；
12.各推进器的推进量计算公式如下：
[0013][0014][0015][0016][0017][0018]
[0019]
其中，l
φ
为任意一个竖向推进器到浅航器中心的距离，l
ψ
为任意一个横向推进器到浅航器中心的距离，i
φ
为绕机体y轴的转动惯量，i
ψ
为绕机体z轴的转动惯量，k1和k2为控制参数，表示浅航器的下降速度，表示浅航器的水平行进速度，tanh为双曲正切饱和函数，表示浅航器机体俯仰角的角速度，表示浅航器机体航向角的角速度。
[0020]
进一步的，浅航器的期望值设定包括以下内容：
[0021]
将浅航器的垂直入坞停泊过程分为机体俯仰下潜阶段、机体上仰阶段及机体竖直平动阶段，将三个阶段的期望航向角设为0
°
，期望速度设为浅航器能够实现范围的任意值；在机体俯仰下潜阶段，将期望俯仰角设定为-45
°
，在机体上仰阶段，将期望俯仰角设定为0
°
，在机体竖直平动阶段，将期望俯仰角设定为90
°
。
[0022]
本发明的优点在于：通过控制算法，实现浅航器的自动垂直入坞停泊，为提高浅航器的工况适应能力、提高浅航器的运行便捷性和实用性打下了基础。
附图说明
[0023]
图1为采用实施例算法的实施效果示意图；
[0024]
图2为浅航器六个推进器的配置示意图；
[0025]
图3为浅航器的机体坐标示意图；
[0026]
图4为采用实施例算法进行垂直入坞的执行流程示意图；
[0027]
图5为采用实施例算法的数值算例结果示意图。
具体实施方式
[0028]
以下结合实施例对本发明作进一步详细描述。
[0029]
本实施例提出一种针对浅航器运动控制的垂直入坞停泊算法，以提高浅航器在特殊作业条件下的运动行进、任务执行能力，该算法实施效果见图1所示(右侧为起始点)。本实施例的最终目的在于将运行在水下的浅航器通过水下闭环机动动作，实现在水面的竖直平动行进，以完成入坞停靠动作。该实施效果中，浅航器机体的水下动作包括可控俯仰下潜阶段、上仰阶段以及竖直平动阶段。
[0030]
本实施例所述浅航器的动力配置如图2所示，包含六个具备正反推进功能的水下推进器，即“竖向推进器1”、“竖向推进器2”、“横向推进器1”、“横向推进器2”、“左-主推进器”及“右-主推进器”，六个推进器的推进量分别以代号t
s1
,t
s2
,t
h1
,t
h2
,tz,ty进行表示，其中s表示“竖”、h表示“横”、z表示“左”、y表示“右”，竖向推进器1和竖向推进器2到机体中心点的距离相同，横向推进器1和横向推进器2到机体中心点的距离相同。为实现如图1所示的效果，需要计算各个推进器的推进量。
[0031]
如图3所示，为本实施例浅航器机体的坐标系示意图，即以浅航器机体长度方向为x轴，宽度方向为y轴，高度方向为z轴。以右手定则为基准，绕x轴转动的角度称为机体横滚角θj，绕y轴转动的角度称为机体俯仰角φj，绕z轴转动的角度称为机体航向角ψj。
[0032]
为计算推进量，需要得到俯仰角的角速度和航向角的角速度由于垂直入坞停泊运动可忽略横滚角运动，所以只需建立俯仰、航向角动态模型如下：
其中，l
φ
为竖向推进器1或竖向推进器2到浅航器中心的距离，l
ψ
为横向推进器1或横向推进器2到浅航器中心的距离，i
φ
为绕机体y轴的转动惯量，i
ψ
为绕机体z轴的转动惯量。即为φj的二阶导数，代表φj的俯仰角的角加速度，即为ψj的二阶导数，代表航向角的角加速度，通过建立俯仰、航向角动态模型，可得出俯仰角和航向角的角速度
[0033]
为实现推进量的计算，还需建立浅航器的深度动态模型和前进距离动态模型，其中浅航器深度动态模型为前进距离动态模型为式中的代表浅航器的下降加速度，为浅航器下降深度d的二阶导数，通过浅航器深度动态模型，可以得出浅航器的下降速度同理，式中的代表浅航器的水平行进加速度，为浅航器水平行进距离l的二阶导数，通过前进距离动态模型可得出浅航器的水平行进速度
[0034]
令期望的俯仰角、航向角分别为φr、ψr，令期望的运行速度为vr，构造以下饱和控制算法：其中k1＞0,k2＞0为控制参数，tanh为双曲正切饱和函数。由于两个竖向推进器的型号是相同的，两个横向推进器的型号是相同的，左右两个主推进器的型号是相同的，通过上述饱和控制算法，即可得出各推进器的推进量计算公式：
[0035][0036][0037][0038][0039][0040][0041]
依据上述推进量的计算公式，通过设定期望的俯仰角、航向角以及速度值，即可实现本实施例所述的垂直入坞停泊控制，执行流程见图4。
[0042]
如图4所示，给期望俯仰角赋值是按照时间序列讯号进行，当时间t＜5s时，浅航器处于俯仰下潜阶段，期望俯仰角设为-45
°
；当时间5s≤t＜10s时，浅航器处于上仰阶段，期
望俯仰角设为0
°
；当时间t≥10s时，浅航器处于竖直平动阶段，期望俯仰角设为0
°
。该过程中，期望航向角始终为0
°
；期望速度始终为vr，该值可在浅航器能够实现的范围内任意选取。当运行到水面，即深度值为0m时(浅航器露出水面)结束算法。如图5所示，是利用本实施例算法的数值算例结果示意图。执行流程中的时间t可根据实际工况进行确定。
[0043]
上述实施例仅用于解释说明本发明的构思，而非对本发明权利保护的限定，凡利用此构思对本发明进行非实质性的改动，均应落入本发明的保护范围。