一种潜航器垂直面航行姿态优化控制方法

1.本发明涉及水下机器人控制领域，尤其涉及一种面向长航程潜航器的控制方法，具体的说是一种潜航器航行姿态优化控制方法。


背景技术：

2.近年来，世界各国对海洋资源的勘探和开发利用越来越重视，且逐渐向深远海发展。潜航器作为探索海洋的重要工具和载体，也向长航程、大深度发展，潜航器的节能减阻技术是增加航程的重要手段，其中潜航器稳态航行时的姿态对阻力影响较大，特别是稳态时攻角和舵角的存在会导致能耗增加、航程缩短。当潜航器在不同深度、不同海域航行时，海洋环境差异大，自身浮力发生变化，潜航器需要一定的攻角和舵角平衡浮力变化引起的外力和外力矩变化。综上，优化潜航器姿态，减小潜航器稳态航行时的攻角和舵角是减小能耗提高航程的关键技术之一，目前长航程潜航器都配有浮力调节装置，并通过检测海水密度、估计浮力等方法调整浮力调节装置，对潜航器参数、装置控制和传感器精度要求较高，适应性不足，也不能实现最优的姿态调节，因此基于浮力调节装置的航行姿态优化控制算法方面尚有不足。


技术实现要素：

3.针对上述长航程潜航器对节能减阻方面的需求以及目前基于浮力调节装置的航行姿态优化控制算法方面尚有不足问题，本发明提出了一种潜航器垂直面航行姿态优化控制方法，实现潜航器稳态航行时零攻角、零舵角控制。
4.本发明为实现上述目的所采用的技术方案是：一种潜航器垂直面航行姿态优化控制方法，包括以下步骤：
5.通过垂直面控制器操纵潜航器各艏艉舵，实现潜航器垂直面的运动控制；
6.通过姿态优化器控制潜航器攻角和舵角，调整浮力调节装置艏部和艉部的质量分布，实现潜航器的姿态控制。
7.所述姿态优化器包括零舵角控制器、零攻角控制器和浮力调节指令分配器；
8.所述零攻角控制器输入为设定的攻角指令及传感器检测的当前攻角值，采用增量式pid算法计算达到攻角指令所需的艏浮力调节装置与艉浮力调节装置的质量之和指令；
9.所述零舵角控制器输入为设定的舵角加权和指令及由传感器检测的各个舵角值的加权和，采用增量式pid算法计算达到舵角加权和指令为零所需的艏浮力调节装置与艉浮力调节装置的质量之差指令；
10.所述浮力调节指令分配器以最小化调节量为目标、以零攻角控制器和零舵角控制器分别输出的浮力调节质量之和指令、质量之差指令为约束条件，采用最优化方法实现零攻角控制器和零舵角控制器输出的综合浮力调节指令到各个浮力调节装置指令的分配。
11.所述浮力调节指令分配器输入为零攻角控制器、零舵角控制器分别输出的浮力调节质量之和指令浮力调节质量之差指令输出为n个艏浮力调节装置的质量指令
m个艏浮力调节装置的质量指令m个艏浮力调节装置的质量指令输入到输出的处理过程转化为有约束二次规划求解问题，待求解的变量为各艏浮力调节装置和艉浮力调节装置的质量指令，目标函数为各艏浮力调节装置和艉浮力调节装置的质量变化量平方和最小，约束条件为各艏浮力调节装置和艉浮力调节装置质量指令之和等于输入的质量之和指令、各艏浮力调节装置和艉浮力调节装置质量指令之差等于输入的质量之差指令。
12.所述浮力调节指令分配器执行以下步骤：
13.待求解变量：
14.目标函数：
15.约束条件：
16.根据约束条件，对目标函数进行最小化，得到最优的n个艏浮力调节装置的质量指令和m个艏浮力调节装置的质量指令
17.所述对目标函数进行最小化采用一阶通用二次规划求解器、内点法、积极集法、智能搜索算法中的任意一种方法进行求解。
18.本发明具有以下优点及有益效果：1.本发明提出了一种潜航器垂直面航行姿态优化控制方法，可实现潜航器稳态航行时零攻角、零舵角控制，可有效降低潜航器阻力，增加航程。
19.2.本发明基于增量式pid形成深度控制器、俯仰角控制器、零攻角控制器、零舵角控制器，采用统一的控制器形式，且充分考虑控制器输出的量程限制和变化量限制，设计简单、性能可靠。
20.3.本发明将浮力调节装置的指令分配问题转化为可通用化的有约束最优化问题，所求分配结果更优且适用于不同规模、不同调节能力的装置条件，具有较好扩展性。
附图说明
21.图1为一种潜航器垂直面航行姿态优化控制方法结构图。
22.图2为一种潜航器垂直面航行姿态优化控制方法仿真结果图。
具体实施方式
23.下面举仿真实例对上述技术方案中描述的方法进行说明：
24.1.一种潜航器垂直面航行姿态优化控制方法，其原理是利用艏部和艉部部署多个浮力调节装置时可改变潜航器质量和质量分布，从而改变潜航器净浮力和浮力力矩，进而影响攻角和舵角的特性，采用优化控制算法实现零攻角、零舵角航行控制；具体为，可通过调整艏部和艉部各浮力调节装置的质量之和改变净浮力，净浮力为垂直方向，若要平衡垂直方向力的变化需要推进器在垂直方向上产生分力，即需要调整潜航器攻角，通过调整浮
力调节装置艏部和艉部的质量分布，即质量之差，可改变潜航器的浮力力矩，进而影响平衡俯仰力矩，进而影响舵角的改变；
25.2.基于上述原理，所述一种潜航器垂直面航行姿态优化控制方法，由执行机构、传感装置、垂直面控制器和姿态优化器组成，各部分组成和工作流程如下：
26.(1)执行机构与传感装置包括：布置在潜航器艏部和艉部的若干浮力调节装置，艏艉舵，各控制器所需的深度值、高度值、俯仰角值、攻角值、舵角值、浮力调节装置质量值检测传感器；其中，设艏部配置n个、艉部配置m个浮力调节装置，每个浮力调节装置可通过泵吸入或排出液体，改变潜航器质量，从而调节潜航器的净浮力(即浮力与重力之差)及力矩，各个浮力调节装置接收姿态优化器输出的质量指令，并通过配备的液位传感器和质量控制器，实现质量的检测和质量的控制，质量控制器根据当前质量值与质量指令之差调节泵吸入或排出液体，从而调节装置质量达到质量指令值；各个舵接收垂直面控制器输出的舵角指令，并通过配备的舵角传感器和舵角控制器实现舵角检测和舵角控制；
27.(2)垂直面控制器由深/高度控制器、俯仰角控制器、力矩分配器组成，通过深/高度-俯仰角-力矩分配的串级控制方法，操纵各艏艉舵实现垂直面的运动控制，其中，深/高度控制器接收外部深/高度指令值，根据传感装置检测的当前深/高度值，采用增量式pid算法计算达到给定深/高度指令所需的俯仰角指令；俯仰角控制器以深/高度控制器输出的俯仰角指令为输入，根据传感器检测的当前俯仰角值，采用增量式pid算法计算达到给定俯仰角指令所需的俯仰力矩指令；力矩分配器将俯仰角控制器输出的俯仰力矩指令分配到各个舵，即计算各个舵角指令，由各个舵完成舵角的控制；
28.(3)姿态优化器由零舵角控制器、零攻角控制器和浮力调节指令分配器组成，其中，零攻角控制器输入为设定的攻角指令(即0)及传感器检测的当前攻角值，采用增量式pid算法计算达到零攻角指令所需的艏艉浮力调节装置的质量之和指令；零舵角控制器输入为设定的舵角加权和指令(即0)及由传感器检测的各个舵角值的加权和，采用增量式pid算法计算达到零舵角所需的艏艉浮力调节装置的质量之差指令；浮力调节指令分配器以最小化调节量为目标、以控制器输出的浮力调节质量之和指令、质量之差指令为约束条件，采用最优化方法实现2个控制器输出的综合浮力调节指令(即质量之和、质量之差指令)到各个浮力调节装置指令的最优分配。
29.一种潜航器垂直面航行姿态优化方法中俯仰角控制器、深/高度控制器、零攻角控制器、零舵角控制器采用增量式pid算法，所述增量式pid算法以被控变量(即控制器的输入，如深/高度、攻角)指令值、被控变量当前值为输入，并考虑控制变量的上下限、控制变量(即控制器的输出，如俯仰角指令、艏艉浮力调节装置质量之和)增量上下限，计算控制变量的增量值和控制变量值，具体过程如下：
30.(1)根据被控变量指令值yrk和当前值yk计算当前时刻被控偏差ek31.ek＝yr
k-yk32.(2)根据当前时刻、上一时刻、上上时刻被控偏差ek、e
k-1
、e
k-2
，计算控制变量增量值δuk[0033][0034]
其中k
p
、ki、kd是控制器的参数，分别为比例系数、积分系数和微分系数；dt为控制
器周期；
[0035]
(3)根据控制变量上下限(u
max
、u
min
)、增量值上下限(δu
max
、δu
min
)计算控制变量输出值uk[0036]
增量值δuk与增量值上限δu
max
比较取小，得到δu
′k：δu
′k＝min(δuk,δu
max
)
[0037]
δu
′k与增量值下限δu
max
比较取大，得到δu
″k：δu
″k＝min(δu
′k,δu
max
)
[0038]
根据控制变量上限u
max
，计算u
′k：u
′k＝min(u
k-1
δu
″k,u
max
)
[0039]
根据控制变量下限u
min
，计算输出值uk：uk＝max(u
′k,u
min
)
[0040]
(4)进入k 1时刻，循环执行步骤(1)～(3)至结束；
[0041]
俯仰角控制器、深/高度控制器、零攻角控制器、零舵角控制器基于上述增量式pid算法，计算流程相同，仅需根据不同控制器设置不同的被控变量(即控制器输入)和控制变量(即控制器输出)及上下限值，具体设置如下：
[0042]
(1)深/高度控制器，被控变量为深/高度,控制变量为俯仰角，控制变量上下限可根据操纵性和使用需求设置，如可设置俯仰角上下限为
±
30
°
、增量变化上下限为
±
10
°
；
[0043]
(2)俯仰角控制器，被控变量为俯仰角，控制变量为俯仰力矩，其中俯仰力矩不是直接测量值，俯仰力矩的当前值(mk)、上下限(m
max
、m
min
)、增量上下限(δm
max
、δm
min
)需要根据各个舵的俯仰力矩水动力系数据各个舵的俯仰力矩水动力系数以及舵角当前值舵角上下限舵角上下限舵角增量上下限舵角增量上下限进行计算：
[0044][0045][0046][0047][0048][0049]
其中，角标b和s分别表示艏(bow的首字母)和艉(stern首字母)、角标l和r分别表示左(left首字母)和右(right首字母)，则bl、br、sl、sr分别表示艏左(bowleft首字母)、艏右(bowright首字母)、艉左(sternleft首字母)、艉右(sternright首字母)。
[0050]
(3)零攻角控制器，被控变量为攻角，控制变量为艏艉各浮力调节装置质量之和，其中控制变量不直接测量，控制变量的当前值上下限上下限增量上下限需要根据艏艉各浮力调节装置质量当前值(艏：艉：)、上下限(艏：艉：艏：艉：)、增量上下限(艏：艉：艏：艉：)进行计算：
[0051][0052][0053][0054][0055][0056]
(4)零舵角控制器，被控变量为舵角加权和，控制变量为艏艉各浮力调节装置质量之差，被控变量和控制变量都不直接测量，其中被控变量的当前值根据各个舵角的当前值加权得到，设权值为各个舵的俯仰力矩水动力系数，则被控变量变为俯仰力矩，即计算同(2)中的mk；控制变量的当前值上下限增量上下限需要根据艏艉各浮力调节装置质量当前值(艏：艉：)、上下限(艏：)、上下限(艏：艉：艏：艉：)、增量上下限(艏：艉：艏：艏：艉：)进行计算：
[0057][0058][0059][0060][0061][0062]
力矩分配器输入为俯仰角控制器计算的俯仰力矩指令mr，输出为各个舵角指令输入到输出的处理过程可转化为有约束二次规划求解问题，待求解的变量为舵角指令，目标函数为舵产生的阻力最小，约束条件为各个舵角产生的力矩和等于俯仰力矩指令，具体描述如下：
[0063]
(1)待求解变量：
[0064]
(2)目标函数：其中x
bl
、x
br
、x
sl
、x
sr
为已知的各个舵阻力系数；
[0065]
(3)约束条件：
[0066]
(4)求解方法：可采用osqp(一阶通用二次规划求解器)、内点法、积极集法、智能搜索算法进行求解。
[0067]
浮力调节指令分配器输入为零攻角控制器、零舵角控制器输出的艏艉浮力调节质量之和指令、质量之差指令输出为各个浮力调节装置的质量指令(艏：艉：)，输入到输出的处理过程可转化为有约束二次规划求解问题，待求解的变量为各浮力调节装置质量指令，目标函数为各浮力调节装置质
量变化量平方和最小，约束条件为艏艉各浮力调节装置质量指令之和等于输入的质量之和指令、艏艉质量指令之差等于输入的质量之差指令，具体描述如下：
[0068]
(1)待求解变量：
[0069]
(2)目标函数：
[0070]
(3)约束条件：
[0071]
(4)求解方法：可采用osqp(一阶通用二次规划求解器)、内点法、积极集法、智能搜索算法进行求解。
[0072]
(1)仿真实例潜航器执行机构参数
[0073]
艏部配置2个、艉部配置1个浮力调节装置，即n＝2，m＝1，各个浮力调节装置的上下限为200kg、0kg；具有艏左、艏右、艉左、艉右4个水平舵、1个垂直舵，各个舵上下限为20
°
、-20
°
；具有艉左、艉右2个推进器；
[0074]
(2)仿真运动学-动力学模型
[0075]
采用标准的6自由度非线性水动力及运动学模型，包括12个状态变量：经度(lon)、纬度(lat)、深度(deep)、滚动角(phi)、俯仰角(theta)、航向角(psi)、载体3轴速度(u、v、w)、载体三轴角速度(p、q、r)；
[0076]
(3)初始值
[0077]
状态初值：lon＝120
°
，lat＝40
°
，deep＝100m，phi＝theta＝psi＝0
°
，u＝3m/s，v＝w＝0m/s，p＝q＝r＝0
°
/s；
[0078]
初始净浮力：200kg；
[0079]
初始重量：30t；
[0080]
各执行机构初值：
[0081]
(4)按照上述技术方案构建增量式pid引擎、垂直面控制器、零攻角控制器、零舵角控制器、浮力调节指令分配器，与仿真运动学-动力学模型构成动态仿真环境，进行2000s仿真，其中0s～1500s时深度指令为100m，1500s～2000s时深度指令由100m变为50m，且为对比姿态优化的效果，在0～500s仅有垂直面控制器，500s后加入姿态优化器；
[0082]
(5)仿真结果分析
[0083]
如图2所示仿真结果图，由于存在初始净浮力，在前500s时仅有垂直面控制器，未加入姿态优化器，在稳态时存在约3
°
俯仰角(定深航行稳态时攻角＝俯仰角)和1.2
°
舵角，500s后加入姿态优化器，开始调节浮力调节装置，至1100s时达到稳定，此时攻角(约等于俯仰角)、舵角皆约为0
°
，1500s开始变深，在变深过程中姿态优化器不工作，变深结束后再次调整攻角和舵角为零。综上本发明所述潜航器垂直面姿态优化方法能有效优化潜航器稳态攻角和舵角，使其为零，从而减小阻力增加航程。