CN111338206B - 一种水下航行器的自动平衡方法及系统 - Google Patents

Abstract

本发明提供了一种水下航行器的自动平衡方法及系统，该自动平衡方法包括参数获取步骤、最优化控制参数计算步骤、设计步骤、推力计算步骤和输出步骤。本发明的有益效果是：本发明在经典PID控制器的基础上，采用了PI智能线性控制器+IDA微分先行算法控制器，并结合PID参数自适应调整方法，形成专用于水下航行器的自动平衡运动控制系统，可以适用于等于或超过5个自由度（5自由度，6自由度）的水下航行器的自动平衡稳定控制和闭环反馈，可根据水下航行器的实时状态参数进行自动机体平衡和航行姿态稳定。

Description

一种水下航行器的自动平衡方法及系统

技术领域

本发明涉及水下航行器技术领域，尤其涉及一种水下航行器的自动平衡方法及系统。

背景技术

PID运动控制技术和算法是一种基于反馈的概念以减少不确定性的控制方法与策略，它是目前在工程实际中应用最为广泛的控制调节器。PID控制器(比例-积分-微分控制器)是一个在工业控制应用中常见的反馈回路部件，由比例单元P、积分单元I和微分单元D组成。PID控制的基础是比例控制；积分控制可消除稳态误差，但可能增加超调；微分控制可加快大惯性系统响应速度以及减弱超调趋势。目前水下机器人运动控制中，多数采用普通PID或PI控制器，且一般主要针对单自由度方向上的闭环控制，其中包括定深PID控制器，负责稳定保持在特定深度上的闭环控制策略；定向PID控制器，负责水下无人机保持特定航向航行的闭环控制策略；姿态稳定PID控制器则负责维持水下无人机机器姿态平稳的闭环控制。

在目前的水下机器人闭环控制策略中，5自由度以及6自由度的水下机器人大多数采用经典的PID控制器，通过多个单独的子PID控制器，独立实现诸如定升沉、定艏摇、定横摇、定纵倾、定横移、定纵荡的闭环反馈控制功能；各个单独的子控制器通过耦合为水下机器人整体控制系统。

但是采用以上所述的子PID控制器组合成为整体控制系统的方法，对水下机器人在不同水流环境及操控要求下有一定的局限性，及所有子PID控制器的参数在系统设计完成时为固定值不可更改，在不同的环境和操控要求下，比较容易由于参数的不适应导致闭环反馈不足或者超调等情况，导致闭环进程缓慢或者稳定性的震荡，使得控制方法和系统效率低下且效果欠佳。同时普通的PID控制方法对于水下机器人的适用性不强，闭环反馈控制效果有待优化。

发明内容

本发明提供了一种水下航行器的自动平衡方法，包括如下步骤：

步骤1，参数获取步骤：获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率；

步骤2，最优化控制参数计算步骤：根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数；

步骤3，设计步骤：将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，根据水下航行器的各个自由度对应的合力/合力矩设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成；

步骤4，推力计算步骤：将合力/合力矩转换为水下航行器的每个推进器的推力数据；

输出步骤：将推进器的推力数据输出到水下航行器。

作为本发明的进一步改进，在所述推力计算步骤和所述输出步骤之间还包括执行步骤5，饱和限制步骤；

步骤5，饱和限制步骤：每个推进器推力做饱和限制，当有推进器达到推力输出极限后，通过调整其他未达到极限的推进器的推力输出，弥补并优化自动平衡控制的效果。

本发明还提供了一种水下航行器的自动平衡系统，包括：

参数获取模块：用于获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率；

最优化控制参数计算模块：用于根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数；

设计模块：用于将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，根据水下航行器的各个自由度对应的合力/合力矩设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成；

推力计算模块：用于将合力/合力矩转换为水下航行器的每个推进器的推力数据；

输出模块：用于将推进器的推力数据输出到水下航行器。

本发明的有益效果是：本发明在经典PID控制器的基础上，采用了PI智能线性控制器+IDA微分先行算法控制器，并结合PID参数自适应调整方法，形成专用于水下航行器的自动平衡运动控制系统，可以适用于等于或超过5个自由度(5自由度，6自由度)的水下航行器的自动平衡稳定控制和闭环反馈，可根据水下航行器的实时状态参数进行自动机体平衡和航行姿态稳定。

附图说明

图1是本发明的流程图；

图2是本发明的PI智能线性控制器的功能实现图；

图3是本发明的IDA微分先行算法控制器的功能实现图；

图4是水下航行器六自由度和对应合力/合力矩的示意图；

图5是6个推进器的水下航行器推力分布示意图。

具体实施方式

本发明公开了一种水下航行器的自动平衡方法，本发明在经典PID控制器的基础上，结合PI智能线性控制器+IDA微分先行算法控制器，并结合PID参数自适应调整方法，针对比较典型的六推进器水下航行器，形成水下航行器的自动平衡方法，可以适用于等于或超过5个自由度(5自由度，6自由度)的水下航行器的自动平衡稳定控制和闭环反馈，可根据水下航行器的实时状态参数进行自动机体平衡和航行姿态稳定。

该水下航行器为水下机器人或水下无人机。

如图1所示，本发明的水下航行器的自动平衡方法，包括如下步骤：

步骤1，参数获取步骤：获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差Err＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率。

推进器的实际状态参数C可以根据推进器输出推力时获取。在水下航行器的整体控制系统中，机器的实际状态参数C与期望状态参数R之间的差值是控制系统实现闭环反馈的直接控制对象。实际状态参数C与期望状态参数R之间的差值，定义为偏差Err；偏差Err随时间的变化率定义为EC。在本发明中，主要控制对象为偏差Err和偏差变化率EC。

Err(偏差)＝R(期望状态参数)-C(实际状态参数)。

EC(偏差变化率)＝(Err-Err’)/△t，Err是这个时刻的偏差，Err’是上一个时刻的偏差，EC即为偏差随时间的变化率。

步骤2，最优化控制参数计算步骤：根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数。

在步骤2中，通过对Err和EC的共同评估，可以根据偏差Err和偏差变化率EC的值，对闭环反馈的区间进行分类。定义偏差和偏差变化率的大小，并分别划分进入到3种水平：高，中等，低；定义偏差Err和偏差变化率EC的组合存在以下5种情形：

1.偏差Err高：

Err处于较高水平(EC大小在情形下不考虑)

2.偏差Err中等，偏差变化率EC高：

Err处于中等水平，EC较高；

3.偏差Err中等，偏差变化率EC中等：

Err处于中等水平，EC中等；

4.偏差Err中等，偏差变化率EC低：

Err处于中等水平，EC较低；

5.偏差Err低：

Err处于较低水平(EC大小在情形下不考虑)。

根据以上5种情形，每种情形对应的权重函数分别为：

μ1(Err,EC)＝μ_BErr(Err)；

μ2(Err,EC)＝μ_MErr(Err)^μ_BEC(EC)；

μ_3(Err,EC)＝μ_MErr(Err)^μ_MEC(EC)；

μ_4(Err,EC)＝μ_MErr(Err)^μ_SEC(EC)；

μ_5(Err,EC)＝μ_SErr(Err)；

μ_BErr(Err)是偏差Err高时对应的特定函数:

R是期望状态参数；

μ_MErr(Err)是偏差Err中等时对应的特定函数：

R是期望状态参数；

μ_SErr(Err)是偏差Err低时对应的特定函数：

R是期望状态参数；

μ_BEC(EC)是偏差变化率EC高时对应的特定函数：

μ_MEC(EC)是偏差变化率EC中等时对应的特定函数：

μ_SEC(EC)是偏差变化率EC低时对应的特定函数；

以上5种情形分别对应5个权重函数μ1(Err,EC)～μ5(Err,EC)，每个权重函数的计算均可以通过特定函数公式实现，函数公式在控制系统中会根据水下航行器动力学特性预先设计完成。PID控制参数通过5个权重函数μ1(Err,EC)～μ5(Err,EC)，在针对不同的情形下会自适应的调节优化，自适应参数调节机制如下：

PID控制中的比例参数K_P：

μj(Err,EC)是权重函数；K_Pj是预先设置的针对5种不同情形的5个比例参数。

PID控制中的积分参数K_I：

其中，K_Ij是预先设置的针对5种不同情形的5个积分参数。

PID控制中的微分参数K_D：

其中，K_Dj是预先设置的针对5种不同情形的5个微分参数。

通过以上自适应参数调节机制，能够在不同的偏差Err及偏差变化率ECC的情形下，自适应的调节PID控制参数，使其优化适配，产生最佳的闭环反馈控制。

PI智能线性控制器在传统PID控制器的基础，采用比例参数K_P和积分参数K_I，对偏差Err并结合偏差变化率EC做闭环反馈控制，该PI智能线性控制器功能实现步骤如图2所示。

IDA微分先行算法控制器主要采用微分参数K_D，先于并跳过偏差Err和偏差变化率EC，针对水下航行器的实际状态参数C的变化情况，控制系统的反馈过调及提高自动平衡的稳定性。该IDA微分先行算法控制器的功能实现步骤如图3所示。

步骤3，设计步骤：将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，系统子控制器是本发明的基本功能单元，水下机器人/水下无人机的自由度与之相对应，根据水下航行器的各个自由度设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成，耦合后组成水下机器人/水下无人机的自动平衡系统。

如图4所示，水下航行器的每个自由度都有相对应的合力/合力矩，每个合力/合力矩则通过每个单独的系统子控制器进行控制。系统子控制器通过在每个自由度上的状态参数偏差Err和偏差变化率EC作为对象和输入，经过控制器内部算法，输出为实现水下航行器自动平衡功能所需要的合力/合力矩，即系统子控制器包括：

(1)纵荡子控制器：通过输入沿X轴方向的前后位移量变化量ΔX，反馈输出控制纵荡的合力Fx；

(2)横移子控制器：通过输入沿Y轴方向的左右位移量变化量ΔY，反馈输出控制横移的合力Fy；

(3)升沉子控制器：通过输入沿Z轴方向的上下位移量变化量ΔZ，反馈输出控制升沉的合力Fz；

(4)横摇子控制器：通过输入绕X轴方向的横摇角度变化量

反馈输出控制横摇的合力矩Nx；

(5)纵倾子控制器：通过输入绕Y轴方向的纵倾角度变化量Δβ，反馈输出控制纵倾的合力矩Ny；

(6)艏摇子控制器：通过输入绕Z轴方向的纵倾角度变化量Δα，反馈输出控制艏摇的合力矩Nz。

通过以上系统子控制器耦合后，水下机器人在六自由度上的状态参数变化量作为输入，即可由系统计算得出每个自由度上反馈控制所需的输出合力/合力矩，从而实现水下机器人的自动平衡运动控制。

步骤4，推力计算步骤：将合力/合力矩转换为水下航行器的每个推进器的推力数据。

在步骤4中，获得六自由度所需的各个合力/合力矩后，需要将这些合力/合力矩转换为水下航行器上的每个推进器要提供的准确的推力大小。

如图5所示，水下航行器拥有6个推进器布置：4个垂直推力产生沿Z轴的向上或者向下的推力F1～F4；2个水平推力使得水下航行器可进行平移运动，产生沿X轴的向前或者向后的运动。需要将6个自由度对应的合力/合力矩最终转换为6个推进器的推力F1～F6。本发明通过控制矩阵B将六自由度对应的合力/合力矩转换计算为6个推进器上要产生准确推力数据，实现水下航行器最终的控制：

F1＝B*Fx,F2＝B*Fy,F3＝B*Fz,F4＝B*Nx,F5＝B*Ny,F6＝B*Nz。

步骤5，饱和限制步骤：经过以上步骤后，通过系统最终转换计算出水下航行器实现自动平衡需要的每个推进器的准确推力输出大小；但是每个推进器的推力输出有极限，需要对每个推进器的计算推力输出做饱和限制，不能超过极限。当有推进器达到推力输出极限后，通过调整其他未达到极限的推进器的推力输出，弥补并优化自动平衡控制的效果。

步骤6，输出步骤：将推进器的推力数据输出到水下航行器。

本发明还公开了一种水下航行器的自动平衡系统，包括：

参数获取模块：用于获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率；

最优化控制参数计算模块：用于根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数；

设计模块：用于将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，根据水下航行器的各个自由度对应的合力/合力矩设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成；

推力计算模块：用于将合力/合力矩转换为水下航行器的每个推进器的推力数据；

输出模块：用于将推进器的推力数据输出到水下航行器。

在所述最优化控制参数计算模块中，定义偏差和偏差变化率的大小，并分别划分进入到3种水平：高、中等、低；定义偏差Err和偏差变化率EC的组合存在5种情形，第1种情形：偏差Err高，第2种情形：偏差Err中等、偏差变化率EC高，第3种情形：偏差Err中等、偏差变化率EC中等，第4种情形：偏差Err中等、偏差变化率EC低，第5种情形：偏差Err低；

5种情形分别对应5个权重函数μ1(Err,EC)～μ5(Err,EC)，5个权重函数为：

μ1(Err,EC)＝μ_BErr(Err)；

μ2(Err,EC)＝μ_MErr(Err)^μ_BEC(EC)；

μ3(Err,EC)＝μ_MErr(Err)^μ_MEC(EC)；

μ4(Err,EC)＝μ_MErr(Err)^μ_SEC(EC)；

μ5(Err,EC)＝μ_SErr(Err)；

μ_BErr(Err)是偏差Err高时对应的特定函数:

R是期望状态参数；

μ_MErr(Err)是偏差Err中等时对应的特定函数：

R是期望状态参数；

μ_SErr(Err)是偏差Err低时对应的特定函数：

R是期望状态参数；

μ_BEC(EC)是偏差变化率EC高时对应的特定函数：

μ_MEC(EC)是偏差变化率EC中等时对应的特定函数：

μ_SEC(EC)是偏差变化率EC低时对应的特定函数；

PID控制参数通过5个权重函数μ1(Err,EC)～μ5(Err,EC)，在针对不同的情形下会自适应的调节优化，自适应参数调节机制如下：

PID控制中的比例参数K_P：

μj(Err,EC)是权重函数；K_Pj是预先设置的针对5种不同情形的5个比例参数。

PID控制中的积分参数K_I：

其中，K_Ij是预先设置的针对5种不同情形的5个积分参数。

PID控制中的微分参数K_D：

其中，K_Dj是预先设置的针对5种不同情形的5个微分参数。

通过以上自适应参数调节机制，能够在不同的偏差Err及偏差变化率ECC的情形下，自适应的调节PID控制参数，使其优化适配，产生最佳的闭环反馈控制。

在所述PI智能线性控制器中，输入偏差Err、偏差变化率EC、比例参数K_P和积分参数K_I，比例反馈值＝K_P*Err，积分反馈值＝K_I*∑Err；PI智能线性控制器反馈值＝比例反馈值+积分反馈值。

在所述IDA微分先行算法控制器中，输入实际状态参数C和微分参数K_D，实际状态参数C变化值CC＝现在时刻实际状态参数C-上个时刻实际状态参数C’，微分先行控制器反馈值＝实际状态参数C变化值随时间变化率CC/△t*K_D。

在所述设计模块中，水下航行器的每个合力/合力矩则通过每个单独的系统子控制器进行控制，系统子控制器通过在每个自由度上的状态参数偏差Err和偏差变化率EC作为对象和输入，经过控制器内部算法，输出为实现水下航行器自动平衡功能所需要的合力/合力矩，系统子控制器包括：

纵荡子控制器：通过输入沿X轴方向的前后位移量变化量ΔX，反馈输出控制纵荡的合力Fx；

横移子控制器：通过输入沿Y轴方向的左右位移量变化量ΔY，反馈输出控制横移的合力Fy；

升沉子控制器：通过输入沿Z轴方向的上下位移量变化量ΔZ，反馈输出控制升沉的合力Fz；

横摇子控制器：通过输入绕X轴方向的横摇角度变化量

反馈输出控制横摇的合力矩Nx；

纵倾子控制器：通过输入绕Y轴方向的纵倾角度变化量Δβ，反馈输出控制纵倾的合力矩Ny；

艏摇子控制器：通过输入绕Z轴方向的纵倾角度变化量Δα，反馈输出控制艏摇的合力矩Nz；

在推力计算步骤中，六个推进器的推力F1～F6，F1＝B*Fx,F2＝B*Fy,F3＝B*Fz,F4＝B*Nx,F5＝B*Ny,F6＝B*Nz，B为控制矩阵。

在所述推力计算模块和所述输出模块之间还包括运行饱和限制模块；饱和限制模块：每个推进器推力做饱和限制，当有推进器达到推力输出极限后，通过调整其他未达到极限的推进器的推力输出，弥补并优化自动平衡控制的效果。

本发明在经典PID控制器的基础上，采用了PI智能线性控制器+IDA微分先行算法控制器，并结合PID参数自适应调整方法，形成专用于水下航行器的自动平衡运动控制系统，可以适用于等于或超过5个自由度(5自由度，6自由度)的水下航行器的自动平衡稳定控制和闭环反馈，可根据水下航行器的实时状态参数进行自动机体平衡和航行姿态稳定。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (8)

1.一种水下航行器的自动平衡方法，其特征在于，包括如下步骤：

步骤1，参数获取步骤：获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率；

步骤2，最优化控制参数计算步骤：根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数；

步骤3，设计步骤：将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，根据水下航行器的各个自由度对应的合力/合力矩设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成；

步骤4，推力计算步骤：将合力/合力矩转换为水下航行器的每个推进器的推力数据；

输出步骤：将推进器的推力数据输出到水下航行器；

在所述步骤2，最优化控制参数计算步骤中，定义偏差和偏差变化率的大小，并分别划分进入到3种水平：高、中等、低；定义偏差Err和偏差变化率EC的组合存在5种情形，第1种情形：偏差Err高，第2种情形：偏差Err中等、偏差变化率EC高，第3种情形：偏差Err中等、偏差变化率EC中等，第4种情形：偏差Err中等、偏差变化率EC低，第5种情形：偏差Err低；

5种情形分别对应5个权重函数μ1(Err,EC)～μ5(Err,EC)，5个权重函数为：μ1(Err,EC)＝μ_BErr(Err)；

μ2(Err,EC)＝μ_MErr(Err)^μ_BEC(EC)；

μ3(Err,EC)＝μ_MErr(Err)^μ_MEC(EC)；

μ4(Err,EC)＝μ_MErr(Err)^μ_SEC(EC)；

μ5(Err,EC)＝μ_SErr(Err)；

μ_BErr(Err)是偏差Err高时对应的特定函数:

R是期望状态参数；

μ_MErr(Err)是偏差Err中等时对应的特定函数：

R是期望状态参数；

μ_SErr(Err)是偏差Err低时对应的特定函数：

R是期望状态参数；

μ_BEC(EC)是偏差变化率EC高时对应的特定函数：

μ_MEC(EC)是偏差变化率EC中等时对应的特定函数：

μ_SEC(EC)是偏差变化率EC低时对应的特定函数；

PID控制参数通过5个权重函数μ1(Err,EC)～μ5(Err,EC)，在针对不同的情形下会自适应的调节优化，自适应参数调节机制如下：

PID控制中的比例参数K_P：

Uj(Err,EC)是权重函数；K_Pj是预先设置的针对5种不同情形的5个比例参数；

PID控制中的积分参数K_I：

其中，K_Ij是预先设置的针对5种不同情形的5个积分参数；

PID控制中的微分参数K_D：

其中，K_Dj是预先设置的针对5种不同情形的5个微分参数；

通过以上自适应参数调节机制，能够在不同的偏差Err及偏差变化率ECC的情形下，自适应的调节PID控制参数，使其优化适配，产生最佳的闭环反馈控制。

2.根据权利要求1所述的自动平衡方法，其特征在于：在所述PI智能线性控制器中，输入偏差Err、偏差变化率EC、比例参数K_P和积分参数K_I，比例反馈值＝K_P*Err，积分反馈值＝K_I*∑Err；PI智能线性控制器反馈值＝比例反馈值+积分反馈值。

3.根据权利要求2所述的自动平衡方法，其特征在于：在所述IDA微分先行算法控制器中，输入实际状态参数C和微分参数K_D，实际状态参数C变化值CC＝现在时刻实际状态参数C-上个时刻实际状态参数C’，微分先行控制器反馈值＝实际状态参数C变化值随时间变化率CC/△t*K_D。

4.根据权利要求3所述的自动平衡方法，其特征在于：在所述步骤3，设计步骤中，水下航行器的每个合力/合力矩则通过每个单独的系统子控制器进行控制，系统子控制器通过在每个自由度上的状态参数偏差Err和偏差变化率EC作为对象和输入，经过控制器内部算法，输出为实现水下航行器自动平衡功能所需要的合力/合力矩，系统子控制器包括：

纵荡子控制器：通过输入沿X轴方向的前后位移量变化量ΔX，反馈输出控制纵荡的合力Fx；

横移子控制器：通过输入沿Y轴方向的左右位移量变化量ΔY，反馈输出控制横移的合力Fy；

升沉子控制器：通过输入沿Z轴方向的上下位移量变化量ΔZ，反馈输出控制升沉的合力Fz；

横摇子控制器：通过输入绕X轴方向的横摇角度变化量

反馈输出控制横摇的合力矩Nx；

纵倾子控制器：通过输入绕Y轴方向的纵倾角度变化量Δβ，反馈输出控制纵倾的合力矩Ny；

艏摇子控制器：通过输入绕Z轴方向的纵倾角度变化量Δα，反馈输出控制艏摇的合力矩Nz。

5.根据权利要求4所述的自动平衡方法，其特征在于：在步骤4，推力计算步骤中，六个推进器的推力F1～F6，F1＝B*Fx,F2＝B*Fy,F3＝B*Fz,F4＝B*Nx,F5＝B*Ny,F6＝B*Nz，B为控制矩阵。

6.根据权利要求1至5任一项所述的自动平衡方法，其特征在于：在所述推力计算步骤和所述输出步骤之间还包括执行步骤5，饱和限制步骤；步骤5，饱和限制步骤：每个推进器推力做饱和限制，当有推进器达到推力输出极限后，通过调整其他未达到极限的推进器的推力输出，弥补并优化自动平衡控制的效果。

7.一种水下航行器的自动平衡系统，其特征在于，包括：

参数获取模块：用于获取推进器的实际状态参数C和期望状态参数R，计算偏差和偏差变化率，偏差用Err表示，偏差变化率用EC表示，偏差＝期望状态参数R-实际状态参数C，偏差变化率是偏差随时间的变化率；

最优化控制参数计算模块：用于根据偏差和偏差变化率的值，进行自适应参数调节，优化PID控制参数，得到最优化控制参数；

设计模块：用于将PI智能线性控制器和IDA微分先行算法控制器组合成为系统子控制器，根据水下航行器的各个自由度对应的合力/合力矩设计对应的系统子控制器，水下航行器的每个自由度的控制由单独的系统子控制器完成；

推力计算模块：用于将合力/合力矩转换为水下航行器的每个推进器的推力数据；

输出模块：用于将推进器的推力数据输出到水下航行器；

在所述最优化控制参数计算模块中，定义偏差和偏差变化率的大小，并分别划分进入到3种水平：高、中等、低；定义偏差Err和偏差变化率EC的组合存在5种情形，第1种情形：偏差Err高，第2种情形：偏差Err中等、偏差变化率EC高，第3种情形：偏差Err中等、偏差变化率EC中等，第4种情形：偏差Err中等、偏差变化率EC低，第5种情形：偏差Err低；

5种情形分别对应5个权重函数μ1(Err,EC)～μ5(Err,EC)，5个权重函数为：μ1(Err,EC)＝μ_BErr(Err)；

μ2(Err,EC)＝μ_MErr(Err)^μ_BEC(EC)；

μ3(Err,EC)＝μ_MErr(Err)^μ_MEC(EC)；

μ4(Err,EC)＝μ_MErr(Err)^μ_SEC(EC)；

μ5(Err,EC)＝μ_SErr(Err)；

μ_BErr(Err)是偏差Err高时对应的特定函数:

R是期望状态参数；

μ_MErr(Err)是偏差Err中等时对应的特定函数：

R是期望状态参数；

μ_SErr(Err)是偏差Err低时对应的特定函数：

R是期望状态参数；

μ_BEC(EC)是偏差变化率EC高时对应的特定函数：

μ_MEC(EC)是偏差变化率EC中等时对应的特定函数：

μ_SEC(EC)是偏差变化率EC低时对应的特定函数；

PID控制参数通过5个权重函数μ1(Err,EC)～μ5(Err,EC)，在针对不同的情形下会自适应的调节优化，自适应参数调节机制如下：

PID控制中的比例参数K_P：

Uj(Err,EC)是权重函数；K_Pj是预先设置的针对5种不同情形的5个比例参数；

PID控制中的积分参数K_I：

其中，K_Ij是预先设置的针对5种不同情形的5个积分参数；

PID控制中的微分参数K_D：

其中，K_Dj是预先设置的针对5种不同情形的5个微分参数；

通过以上自适应参数调节机制，能够在不同的偏差Err及偏差变化率ECC的情形下，自适应的调节PID控制参数，使其优化适配，产生最佳的闭环反馈控制；

在所述PI智能线性控制器中，输入偏差Err、偏差变化率EC、比例参数K_P和积分参数K_I，比例反馈值＝K_P*Err，积分反馈值＝K_I*∑Err；PI智能线性控制器反馈值＝比例反馈值+积分反馈值；

在所述IDA微分先行算法控制器中，输入实际状态参数C和微分参数K_D，实际状态参数C变化值CC＝现在时刻实际状态参数C-上个时刻实际状态参数C’，微分先行控制器反馈值＝实际状态参数C变化值随时间变化率CC/△t*K_D；

在所述设计模块中，水下航行器的每个合力/合力矩则通过每个单独的系统子控制器进行控制，系统子控制器通过在每个自由度上的状态参数偏差Err和偏差变化率EC作为对象和输入，经过控制器内部算法，输出为实现水下航行器自动平衡功能所需要的合力/合力矩，系统子控制器包括：

纵荡子控制器：通过输入沿X轴方向的前后位移量变化量ΔX，反馈输出控制纵荡的合力Fx；

横移子控制器：通过输入沿Y轴方向的左右位移量变化量ΔY，反馈输出控制横移的合力Fy；

升沉子控制器：通过输入沿Z轴方向的上下位移量变化量ΔZ，反馈输出控制升沉的合力Fz；

横摇子控制器：通过输入绕X轴方向的横摇角度变化量

反馈输出控制横摇的合力矩Nx；

纵倾子控制器：通过输入绕Y轴方向的纵倾角度变化量Δβ，反馈输出控制纵倾的合力矩Ny；

艏摇子控制器：通过输入绕Z轴方向的纵倾角度变化量Δα，反馈输出控制艏摇的合力矩Nz；

在推力计算步骤中，六个推进器的推力F1～F6，F1＝B*Fx,F2＝B*Fy,F3＝B*Fz,F4＝B*Nx,F5＝B*Ny,F6＝B*Nz，B为控制矩阵。

8.根据权利要求7所述的自动平衡系统，其特征在于：在所述推力计算模块和所述输出模块之间还包括运行饱和限制模块；

饱和限制模块：每个推进器推力做饱和限制，当有推进器达到推力输出极限后，通过调整其他未达到极限的推进器的推力输出，弥补并优化自动平衡控制的效果。

Images