一种潜航器最优容错自适应控制分配方法与流程

1.本发明涉及水下机器人行为优化控制领域，尤其涉及一种面向长航程潜航器的行为优化控制方法，具体的说是一种潜航器最优容错自适应控制分配方法。


背景技术：

2.近年来，海洋开发日趋重要，潜航器是探索海洋的重要工具，并逐渐向长航程、大深度发展，潜航器的降低阻力是增加航程的重要手段，其中根据各个执行机构的阻力系数，将控制器输出的控制力合理分配到各执行机构可有效降低阻力，而当前很多潜航器都是根据经验或简单计算对各个执行机构采取固定比例或固定方式操作，如：利用水平舵调节俯仰控制指令且某些航速下使用艉舵、某些情况前后舵按一定比例都使用等，这些使用方式都没有达到最小化阻力时的分配，且执行机构故障或性能退化时不能主动适应，本发明即针对此问题提出一种潜航器最优容错自适应控制分配方法。


技术实现要素：

3.针对上述长航程潜航器对减阻方面的需求以及目前实际应用中控制分配不能达到最优且适应性不足等问题，本发明提出了一种潜航器最优容错自适应控制分配方法，实现阻力最小下的最优自适应控制分配。
4.本发明为实现上述目的所采用的技术方案是：一种潜航器最优容错自适应控制分配方法，包括以下步骤：
5.将控制器输出的综合力和力矩信息构成输入变量，各个执行机构指令值构成输出变量，以航行阻力最小为目标，根据输入变量和输出变量构建约束条件，将输入变量分配到各个输出变量，进而控制各执行机构动作。
6.所述以航行阻力最小为目标，根据输入变量和输出变量构建约束条件，将输入变量分配到各个输出变量，包括以下步骤：
7.1)基于执行机构指令上下限约束、执行机构指令变化量上下限约束、第一目标函数、松弛后的综合力和力矩约束、松弛变量取值范围约束构建基于约束松弛的可行解判定阶段的最优化问题；
8.2)将最小化第一目标函数所求出的各变量的最优解带入松弛后的综合力和力矩约束；
9.3)根据第二目标函数、执行机构指令上下限约束公式、执行机构指令变化量上下限约束、松弛后的综合力和力矩约束构建以阻力最小为目标的最优分配求解阶段的最优化问题，以极小化第二目标函数求解出各执行机构的最优值，发送至各执行机构。
10.所述输入变量包括综合推力f
sum
、综合俯仰力矩m
sum
、综合横滚力矩k
sum
、综合偏航力矩n
sum
。
11.所述输出变量包括左推进器转速x
nl
、右推进器转速x
nr
、艏部左舵偏角x
δbl
、艏部右舵偏角x
δbr
、艉部左舵偏角x
δsl
、艉部右舵偏角x
δsr
、艉部垂直舵偏角x
δr
。
12.所述执行机构指令上下限约束：
[0013][0014]
其中，上标为l的量表示相应执行机构指令下限；上标为u的表示相应执行机构指令上限；
[0015]
所述执行机构指令变化量上下限约束：
[0016][0017]
其中，前缀δ表示相应执行机构变化量，上标为l的量表示相应执行机构指令变化量下限；上标为u的量表示相应执行机构指令变化量上限；上标为t的量表示相应执行机构指令当前值，即t时刻值。
[0018]
所述第一目标函数：
[0019][0020]
其中，j表示函数值，c
fl
、c
fu
、c
ml
、c
mu
、c
nl
、c
nu
、c
kl
、c
ku
分别为各松弛量前的系数；各松弛量包括：综合推力下限松弛量ε
fl
、综合推力上限松弛量ε
fu
、综合俯仰力矩下限松弛量ε
ml
综合俯仰力矩上限松弛量ε
mu
、综合偏航力矩下限松弛量ε
nl
、综合偏航力矩上限松弛量ε
nu
、综合横滚力矩下限松弛量ε
kl
、综合横滚力矩上限松弛量ε
ku
。
[0021]
所述松弛后的综合力和力矩约束如下：
[0022][0023]
其中，η为推进器系数；l
nl
、l
nr
分别为左、右推进器偏航力臂；左推进器转速x
nl
、右推进器转速x
nr
、艏部左舵偏角x
δbl
、艏部右舵偏角x
δbr
、艉部左舵偏角x
δsl
、艉部右舵偏角x
δsr
、艉部垂直舵偏角x
δr
；各松弛量包括：综合推力下限松弛量ε
fl
、综合推力上限松弛量ε
fu
、综合俯仰力矩下限松弛量ε
ml
综合俯仰力矩上限松弛量ε
mu
、综合偏航力矩下限松弛量ε
nl
、综合偏航力矩上限松弛量ε
nu
、综合横滚力矩下限松弛量ε
kl
、综合横滚力矩上限松弛量ε
ku
。
[0024]
所述第二目标函数如下：
[0025]
j＝(d
δbl
·
x
δbl
)2 (d
δbr
·
x
δbr
)2 (d
δsl
·
x
δsl
)2 (d
δsr
·
x
δsl
)2 (d
δr
·
x
δr
)2ꢀꢀ
(1)
[0026]
其中，j表示函数值，d
δbl
、d
δbr
、d
δsl
、d
δsr
、d
δr
分别为艏部左舵、艏部右舵、艉部左舵、艉部右舵、艉部垂直舵阻力系数；艏部左舵偏角x
δbl
、艏部右舵偏角x
δbr
、艉部左舵偏角x
δsl
、艉部右舵偏角x
δsr
、艉部垂直舵偏角x
δr
。
[0027]
所述约束松弛的可行解判定阶段和以阻力最小为目标的最优分配求解阶段采用一阶通用二次规划求解器、内点法、积极集法、智能搜索算法中的任意一种方法进行求解。
[0028]
本发明具有以下优点及有益效果：
[0029]
1.本发明提出了一种潜航器最优容错自适应控制分配方法，可实现潜航器控制器输出的综合力合力矩到各个执行机构的最优分配，可有效降低潜航器阻力，增加航程。
[0030]
2.本发明将原问题分解为两阶段在线求解的策略，可保证每个求解周期都具有可行解。
[0031]
3.本发明可根据传感器采集的执行机构性能信息实时调整各执行机构上下限、执行机构变化量上下限，从而自动适应执行机构故障或性能退化的情况。
附图说明
[0032]
图1为一种潜航器最优容错自适应控制分配方法结构图。
[0033]
图2为一种潜航器最优容错自适应控制分配方法仿真结果经纬度图。
[0034]
图3为一种潜航器最优容错自适应控制分配方法仿真结果艏舵图。
[0035]
图4为一种潜航器最优容错自适应控制分配方法仿真结果艉舵图。
[0036]
图5为一种潜航器最优容错自适应控制分配方法仿真结果垂直舵图。
具体实施方式
[0037]
下面举仿真实例对上述技术方案中描述的方法进行说明。
[0038]
如图1所示，一种潜航器最优容错自适应控制分配方法，待求解问题描述为：
[0039]
输入变量为控制器输出的综合力和力矩信息，输出变量为各个执行机构指令值，以航行阻力最小为目标，以综合力合力矩、执行机构指令上下限、执行机构指令变化量上下限为约束条件，将输入变量分配到各个输出变量；
[0040]
所述控制分配方法的控制对象，即潜航器，以具有艉部左推进器、艉部右推进器、
艏部左舵、艏部右舵、艉部左舵、艉部右舵、艉部垂直舵共7个执行机构为例，则所述最优化问题数学描述为以下5部分并定义为问题1：
[0041]
(1)待求解变量：左推进器转速x
nl
、右推进器转速x
nr
、艏部左舵偏角x
δbl
、艏部右舵偏角x
δbr
、艉部左舵偏角x
δsl
、艉部右舵偏角x
δsr
、艉部垂直舵偏角x
δr
[0042]
(2)目标函数：
[0043]
j＝(d
δbl
·
x
δbl
)2 (d
δbr
·
x
δbr
)2 (d
δsl
·
x
δsl
)2 (d
δsr
·
x
δsl
)2 (d
δr
·
x
δr
)2ꢀꢀ
(1)
[0044]
其中，d
δbl
、d
δbr
、d
δsl
、d
δsr
、d
δr
分别为艏部左舵、艏部右舵、艉部左舵、艉部右舵、艉部垂直舵阻力系数；
[0045]
(3)综合力和力矩约束：
[0046][0047]
其中，f
sum
、m
sum
、k
sum
、n
sum
分别为综合推力、综合俯仰力矩、综合横滚力矩、综合偏航力矩；m
δbl
、m
δbr
、m
δsl
、m
δsr
分别为艏部左舵、艏部右舵、艉部左舵、艉部右舵俯仰力矩系数；n
δr
为艉部垂直舵偏航力矩系数；k
δbl
、k
δbr
、k
δsl
、k
δsr
、k
δr
分别为艏部左舵、艏部右舵、艉部左舵、艉部右舵、艉部垂直舵横滚力矩系数；η推进器系数；l
nl
、l
nr
分别为左、右推进器偏航力臂；
[0048]
(4)执行机构指令上下限约束：
[0049][0050]
其中，上标为l的量表示相应执行机构指令下限；上标为u的表示相应执行机构指令上限；
[0051]
(5)执行机构指令变化量上下限约束：
[0052][0053]
其中，前缀δ表示相应执行机构变化量，上标为l的量表示相应执行机构指令变化量下限；上标为u的表示相应执行机构指令变化量上限；上标t的量表示相应执行机构指令当前值，即t时刻值；
[0054]
所述优化问题数学描述直接求解有无解的可能性，由于在线控制需要每个周期都给出解，因此不能直接求解上述最优化问题，所述最优化问题转化为两个约束优化求解阶段：基于约束松弛的可行解判定阶段和以阻力最小为目标的最优分配求解阶段：
[0055]
所述基于约束松弛的可行解判定阶段将控制器输出的综合力和力矩约束条件，即公式(2)，进行松弛，将执行机构指令值和松弛变量共同作为决策变量，以松弛变量平方和最小为目标进行求解，获取松弛后的约束条件，由于一般综合力合力矩根据各执行机构的上下限进行了限幅，因此松弛后的约束条件存在可行解，该问题数学描述如下，并定为问题2：
[0056]
(1)待求解变量：左推进器转速x
nl
、右推进器转速x
nr
、艏部左舵偏角x
δbl
、艏部右舵偏角x
δbr
、艉部左舵偏角x
δsl
、艉部右舵偏角x
δsr
、艉部垂直舵偏角x
δr
、综合推力下限松弛量ε
fl
、综合推力上限松弛量ε
fu
、综合俯仰力矩下限松弛量ε
ml
综合俯仰力矩上限松弛量ε
mu
、综合偏航力矩下限松弛量ε
nl
、综合偏航力矩上限松弛量ε
nu
、综合横滚力矩下限松弛量ε
kl
、综合横滚力矩上限松弛量ε
ku
[0057]
(2)目标函数：
[0058][0059]
其中，各松弛量前的系数为加权系数，用于调整各约束条件的重要性；
[0060]
(3)松弛后的综合力和力矩约束：
[0061][0062]
(4)执行机构指令上下限约束同公式(3)；
[0063]
(5)执行机构指令变化量上下限约束同公式(4)；
[0064]
(6)松弛变量取值范围约束：
[0065]
ε
fl
，ε
fu
，ε
ml
，ε
mu
，ε
nl
，ε
nu
，ε
kl
，ε
ku
≥0 (7)
[0066]
所述以阻力最小为目标的最优分配求解阶段将松弛后的约束条件作为约束条件，求解各执行机构的最优指令值，从而实现潜航器减阻节能航行，且对一定程度的执行机构故障或性能退化情况具有自适应能力，该问题数学描述如下，并定为问题3：
[0067]
(1)待求解变量：x
nl
、x
nr
、x
δbl
、x
δbr
、x
δsl
、x
δsr
、x
δr
[0068]
(2)目标函数同公式(1)；
[0069]
(3)松弛后的综合力和力矩约束：
[0070][0071]
其中为问题2中各个待求解变量的最优解；
[0072]
(4)执行机构指令上下限约束同公式(3)；
[0073]
(5)执行机构指令变化量上下限约束同公式(4)；
[0074]
上述求解过程对执行机构部分故障或性能退化时所述方法具有一定自适应能力，具体为：各执行机构故障或性能退化表现为各执行机构操作范围变小，即上下限或变化量上下限所包含的区间变小，如执行机构卡死时可将上下限都设置为卡死值。
[0075]
所述约束松弛的可行解判定阶段和以阻力最小为目标的最优分配求解阶段采用一阶通用二次规划求解器、内点法、积极集法、智能搜索算法中的任意一种方法进行求解。
[0076]
1.一种潜航器最优容错自适应控制分配方法，其基本原始是将控制分配问题转化为最优化问题，为保证在线求解的可行性，将该最优化问题分解为2个阶段：基于约束松弛的可行解判定阶段和以阻力最小为目标的最优分配求解阶段；
[0077]
2.以具有艉部左推进器、艉部右推进器、艏部左舵、艏部右舵、艉部左舵、艉部右舵、艉部垂直舵共7个执行机构为例，对上述原理进行说明：
[0078]
(1)控制器根据给定的控制目标(包括：速度指令、航向指令、深度指令、横滚指令)和被控变量当前测量值计算出达到目标所需的综合力和力矩(包括：推力、偏航力矩、俯仰力矩、横滚力矩)；
[0079]
(2)基于公式(3)、(4)、(5)、(6)、(7)构建基于约束松弛的可行解判定阶段的最优化问题；
[0080]
(3)将步骤(1)中最小化公式(5)所求出的公式(5)中各变量的最优解带入公式(8)；
[0081]
(4)根据公式(1)、(3)、(4)、(8)构建以阻力最小为目标的最优分配求解阶段的最优化问题，以极小化公式(1)求解出各执行机构的最优值，发送至各执行机构，改变潜航器状态并逐步趋向控制目标；
[0082]
(5)重复步骤(1)～(4)，最终潜航器达到控制目标；
[0083]
(6)步骤(1)和(2)的求解方法：可采用osqp(一阶通用二次规划求解器)、内点法、积极集法、智能搜索算法进行求解。
[0084]
3.仿真实例对上述方法进行验证
[0085]
(1)仿真运动学-动力学模型
[0086]
采用标准的6自由度非线性水动力及运动学模型，包括12个状态变量：经度(lon)、纬度(lat)、深度(deep)、滚动角(phi)、俯仰角(theta)、航向角(psi)、载体3轴速度(u、v、w)、载体三轴角速度(p、q、r)；
[0087]
(2)初始值
[0088]
状态初值：lon＝115.17
°
，lat＝17.048
°
，deep＝700m，phi＝theta＝0
°
，psi＝30
°
，u＝3m/s，v＝w＝0m/s，p＝q＝r＝0
°
/s；
[0089]
初始净浮力：200kg；
[0090]
初始重量：30t；
[0091]
各执行机构初值：执行机构初值：
[0092]
(3)按照上述技术方案结构图构建潜航器控制器模块和控制分配模块，与仿真运动学-动力学模型构成动态仿真环境，潜航器梳形航行250分钟，在120分钟后加入艏部左舵卡死故障，卡死角度为-10
°
；
[0093]
(4)仿真结果分析
[0094]
如图2～图5所示仿真结果图，所述最优自适应容错控制分配方法，在动态控制过程中协同合理的分配各个执行机构，使得各执行机构以较小幅度协同动作，减小阻力，在加入舵卡死故障后，仍然能够自适应调整分配策略，保证控制性能。综上本发明所述最优自适应容错控制分配方法可通过将控制器的综合力和力矩指令最优分配到各个执行机构，从而减小阻力、增加航程。