一种水下航行器姿态控制方法和系统

1.本发明涉及水下航行器领域，尤其涉及水下航行器姿态控制方法和系统。


背景技术：

2.水下航行器的运动控制是完成海洋水文调查和近海巡航等任务的技术前提，欠驱动水下航行器具有成本低、体积小、重量轻、灵活性强、可靠度高等多方面优势，而其姿态控制则是运动控制的重要基础。由于海洋环境复杂多变，水下航行器运动过程中受海浪、海流及海底地形等因素干扰，导致水动力参数及相关非线性阻尼参数不确定，水下航行器的空间运动模型具有非线性、强耦合性且参数可变等特点，控制系统设计难度较高，对控制精度、性能稳定及鲁棒性等要求都很高。
3.随着水下航行器日益受到关注，近年来许多控制方法都应用到了水下航行器的运动控制中。
4.例如，一种模糊控制与滑模控制相结合的水下航行器的姿态控制器，抑制了控制器输出的抖振且具有鲁棒性，但该控制器的设计是基于水下航行器的航向系统动力学模型线性化处理，缺乏实际可行性。一种水下航行器的航向系统动力学模型提供了一种非线性全状态反馈律，实现了水下航行器的航向控制，但其未考虑模型的不确定性和外界干扰的影响。一种增益调参控制方法将水下航行器模型线性化处理，简化了控制器的设计，但该方法仅在操作点的较小领域内有效。一种反馈线性化方法是利用全状态反馈抵消水下航行器航向系统动力学模型中的非线性，使新航向系统的输入输出间具有线性关系，但是这种方法对研究对象的动力学模型具有较高的精度要求。一种基于神经网络方法的动态逆控制可以在水下航行器很难获得精确的动力学模型的情况下，采用神经网络进行系统辨识，但其计算复杂度高且收敛性很难得到保证。
5.上述方法的性能在很大程度上依赖于航向系统动力学模型的精度，但在实际的应用中，由于多种环境干扰很难获取准确的参数，模型的精度很难得到保证，性能的稳定性及鲁棒性难以满足控制要求。


技术实现要素：

6.为了解决上述欠驱动水下航行器水下动力性能参数不确定和对外部干扰敏感，难以满足控制要求的问题，本技术的实施例提供了一种水下航行器姿态控制方法及装置。
7.第一方面，本技术的实施例提供一种水下航行器姿态控制方法，所述方法包括：设置控制对象的控制目标的值；所述控制对象包括舵机，所述控制目标至少包括以下之一：俯仰角、横滚角和偏航角；获得所述控制目标的至少一个过渡信号；根据所述控制对象输出的第一姿态计算姿态估计值和扰动量估计值；所述控制对象输出的第一姿态包括所述控制对象输出的第一俯仰角、第一横滚角和第一偏航角；所述姿态估计值至少包括俯仰角估计值、横滚角估计值和偏航角估计值；计算所述过渡信号与姿态估计值的误差值，对所述误差值进行非线性误差反馈，得出第一控制量；根据所述扰动量的估计值对第一控制量进行补偿，
获得第二控制量；根据所述第二控制量获得所述控制对象输出的第二姿态，所述控制对象输出的第二姿态包括所述控制对象输出的第二俯仰角、第二横滚角和第二偏航角。
8.在一种实施方式中，所述获得所述控制目标的至少一个过渡信号，包括：根据所述控制目标的值获得过渡信号和所述过渡信号的微分信号。
9.在一种实施方式中，所述根据所述控制对象输出的第一姿态计算姿态估计值和扰动量估计值，包括：根据所述过渡信号和所述过渡信号的微分信号获得所述控制对象输出的第一姿态；根据所述控制对象输出的第一姿态获得所述姿态的估计值和扰动量估计值；所述姿态的估计值包括第一估计值和第二估计值，所述第二估计值为所述第一估计值的微分值。
10.在一种实施方式中，所述计算所述过渡信号与姿态估计值的误差值，对所述误差值进行非线性误差反馈，得出第一控制量，包括：将所述过渡信号和所述第一估计值进行比较，得到第一误差值；将所述过渡信号的微分信号和所述第二估计值进行比较，得到第二误差值；对所述第一误差值和第二误差值进行非线性误差反馈，得出所述第一控制量；。
11.在一种实施方式中，所述根据所述扰动量来对第一控制量进行补偿，获得第二控制量，包括：将所述扰动量反馈到所述第一控制量，利用补偿系数对所述第一控制量进行补偿，输出第二控制量。
12.在一种实施方式中，所述第一控制量包括俯仰角的第一控制量、横滚角的第一控制量和偏航角第一控制量；所述第二控制量包括俯仰角的第二控制量、横滚角的第二控制量和偏航角第二控制量。
13.在一种实施方式中，在所述设置控制对象的控制目标的值之前，还包括：建立所述控制对象的地面坐标系和载体坐标系；确定所述控制对象在地面坐标系下的运动学模型；确定所述控制对象在所沪载体坐标系下水下航行器的姿态参数的模型；根据所述运动学模型和姿态参数的模型，确定所述水下航行器的动力学模型。
14.第二方面，本技术的实施例提供一种水下航行器姿态控制系统，所述装置包括：目标设置模块，用于根据水下航行器的动力学模型设置控制对象的控制目标；所述控制对象包括舵机，所述控制目标至少包括以下之一：俯仰角、横滚角和偏航角；跟踪微分器，用于获得所述控制目标的至少一个过渡信号；扩张状态观测器，用于根据所述控制对象输出的第一姿态计算姿态估计值和扰动量估计值；所述控制对象输出的第一姿态包括所述控制对象输出的第一俯仰角、第一横滚角和第一偏航角；所述姿态估计值至少包括俯仰角估计值、横滚角估计值和偏航角估计值；非线性误差反馈模块，用于计算所述过渡信号与姿态估计值的误差值，对所述误差值进行非线性误差反馈，得出第一控制量；扰动补偿模块，用于根据所述扰动量的估计值对所述第一控制量进行补偿，获得第二控制量；和控制对象，用于根据所述第二控制量获得所述控制对象输出的第二姿态，所述控制对象输出的第二姿态包括所述控制对象输出的第二俯仰角、第二横滚角和第二偏航角。
15.在一种实施方式中，所述舵机包括俯仰舵机、横滚舵机和偏航舵机。
16.本技术的实施例提出一种水下航行器姿态控制方法，基于自抗扰控制器(active disturbance rejection control，adrc)为期望姿态安排过渡过程，通过扩张状态观测器对内扰和外扰进行估计并实时补偿，实现了对横滚角俯仰角θ和偏航角ψ三通道姿态进行解耦，能够很好地克服水下航行器的强耦合性、水下动力性能参数不确定性以及水流变
化等外部干扰问题。
17.此外本技术的实施例提出一种水下航行器姿态控制系统，利用非线性状态误差反馈模块来有效抑制跟踪误差。对本技术实施例提供的水下航行器姿态控制系统进行稳定性控制、抗扰性及鲁棒性实验，与串级pid控制系统进行定量对比分析可知，本技术实施例提供的水下航行器姿态控制系统在稳定性、抗干扰性和鲁棒性方面具有明显的优势。
附图说明
18.为了更清楚地说明本说明书披露的多个实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本说明书披露的多个实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
19.下面对实施例或现有技术描述中所需使用的附图作简单地介绍。
20.图1为本技术实施例提出的欠驱动无人水下航行器三维运动的坐标系示意图；
21.图2为本技术实施例提出的欠驱动无人水下航行器舵机模型；
22.图3为本技术实施例提出的自抗扰控制器的结构示意图；
23.图4为本技术实施例提出的水下航行器的姿态控制系统的姿态控制原理图；
24.图5为本技术实施例提出的俯仰角θ稳定性实验仿真结果；
25.图6为本技术实施例提出的横滚角稳定性实验仿真结果；
26.图7为本技术实施例提出的偏航角ψ稳定性实验仿真结果；
27.图8为本技术实施例提出的横滚角抗干扰性实验仿真结果；
28.图9为本技术实施例提出的俯仰角θ抗干扰性实验仿真结果；
29.图10为本技术实施例提出的偏航角ψ抗干扰性实验仿真结果；
30.图11为本技术实施例提出的俯仰角θ鲁棒性实验仿真结果；
31.图12为本技术实施例提出的横滚角鲁棒性实验仿真结果；
32.图13为本技术实施例提出的偏航角ψ鲁棒性实验仿真结果。
具体实施方式
33.在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。
34.在以下的描述中，所涉及的术语“第一\第二\第三等”或模块a、模块b、模块c等，仅用于区别类似的对象，不代表针对对象的特定排序，可以理解地，在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本技术实施例能够以除了在这里图示或描述的以外的顺序实施。
35.在以下的描述中，所涉及的表示步骤的标号，如s110、s120......等，并不表示一定会按此步骤执行，在允许的情况下可以互换前后步骤的顺序，或同时执行。
36.除非另有定义，本文所使用的所有的技术和科学术语与属于本技术的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本技术实施例的目的，不是旨在限制本技术。
37.下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行描述。
38.下面介绍本技术实施例的整体构思。
39.图1为本技术实施例提出的水下航行器三维运动坐标系示意图。为了便于描述水下航行器的三维运动，需要建立地面坐标系{e}和载体坐标系{b}。如图1所示，根据地面坐标系{e}设定[x，y，z]
t
为地面坐标系{e}下水下航行器的位置向量，地面坐标系{e}下的运动学模型为：
[0040][0041]
式(1)中为地面坐标系系{e}下水下航行器x、y、z三个方向的速度向量；[u，v，w]
t
为载体坐标系{b}下水下航行器对应的速度向量，为载体坐标系{b}下水下航行器对应的姿态角向量，姿态向量角包括横滚角俯仰角θ和偏航角ψ。
[0042]
设定[p，q，r]
t
为载体坐标系{b}下水下航行器三个通道角速度向量，则根据式(1)确定载体坐标系{b}下水下航行器的姿态角速度的模型为：
[0043][0044]
根据式(1)和式(2)，水下航行器的动力学模型可以表示为：
[0045][0046]
式(3)中m是水下航行器的质量，s是最大横截面积，ρ是水的密度，l是长度，f是推力，v
f
为水下航行器的推力f方向的航行速度，λ
11
，λ
22
，λ
33
是附加质量，λ
26
，λ
35
是附加静矩,λ
44
，λ
55
，λ
66
是附加惯量；j
x
，j
y
，j
z
动质量；x
g
、y
g
、z
g
是水下航行器的位置向量在地面坐标系{e}体系中的坐标分量，δg是负浮力，g是重力，p
′
、q
′
、r
′
分别是p、q、r对应的无量纲量，其中c
x
(0)、(0)、是水下航行器的水动力参数，其下标x，y，z是粘性类流体力在地面坐标系系{e}体系中的坐标分量，r，m，n是粘性类流体力相对浮心大力矩在地面坐标系系{e}体系的坐标分量；δ
e
、δ
r
、δ
d
分别为水下航行器动力学模型的俯仰通道、横滚通道、偏航通道的控制输入量，其下标e表示升降舵角、r表示方向舵角、d表示差动舵角。
[0047]
水下航行器动力学模型中的控制输入量δ
e
、δ
r
、δ
d
也称为舵角，舵角需要水下航行器的舵机执行实现。将控制俯仰角的舵机记为第一舵机，将控制横滚角的舵机记为第二舵机，将控制偏航角的舵机记为第三舵机。舵角的计算需要通过舵机模型的算法实现。
[0048]
图2为本技术实施例提出的水下航行器姿态控制方法的舵机模型，如图2所示，舵机模型包括角速度控制模块21和舵角控制模块22。角速度控制模块21用于根据输入的控制量v(t)计算获得舵机的角速度v
r
(t)，其传递函数g1(s)为：
[0049][0050]
式中v(s)是控制量v(t)的拉普拉斯变换，v(s)＝l[v(t)]；v
r
(s)是舵机转速v
r
(t)的拉普拉斯变换，v
r
(s)＝l[v
r
(t)]，s表示拉普拉斯算子，t表示采样时间。
[0051]
在现实场景中，舵机为执行机构的主动部分，通常采用电机实现。在电机的负载一定时，电机传递函数g1(s)如下：
[0052][0053]
其中，k为电机的增益系数，t为电机的时间常数。
[0054]
则根据式(4)和式(5)可得：
[0055][0056]
当舵机(电机)的输入量为控制量v(t)时，根据式(6)计算，可得舵机(电机)角速度v
r
(t)。
[0057]
舵角控制模块22用于根据舵机的角速度v
r
(t)计算输出的舵角δ(t)，其传递函数g(s)为：
[0058][0059]
其中，δ(s)是舵角δ(t)的拉普拉斯变换，δ(s)＝l[δ(t)]。
[0060]
理想情况下，舵机输出的舵角可由舵机(电机)角速度积分得到，因此传递函数g1(s)和g(s)的关系为：
[0061][0062]
根据式(7)和式(8)可得：
[0063][0064]
当舵机(电机)的输入为控制量v(t)时，根据式(6)和式(9)联合计算，可得舵机输出的舵角δ(t)。
[0065]
在本技术实施例中，舵机模型也可采用上述式(8)的传递函数进行描述。
[0066]
由于舵机模型的为一阶传递函数，可以通过自抗扰控制器对舵机进行控制。
[0067]
自抗扰控制器的机理在于把作用于控制对象的所有不确定因素归结为扰动量，用控制对象的输入的控制量和输出的舵角数据对扰动量进行估计补偿。控制对象可以是舵机，控制目标可以是舵机输出的舵角，包括俯仰角、横滚角和偏航角的值之一或组合。
[0068]
本技术的实施例提出一种水下航行器姿态控制方法，基于自抗扰控制器(active disturbance rejection control，adrc)为期望姿态安排过渡过程，通过扩张状态观测器对内扰和外扰进行估计并实时补偿，实现了对横滚角俯仰角θ和偏航角ψ三通道姿态进行解耦，能够很好地克服水下航行器的强耦合性、水下动力性能参数不确定性以及水流变化等外部干扰问题。
[0069]
图3为本技术实施例提出的水下航行器姿态控制方法中自抗扰控制器的结构示意图。如图3所示，自抗扰控制器主要由跟踪微分器td、扩张状态观测器eso、非线性误差反馈nlsef模块以及扰动补偿模块四部分构成。
[0070]
跟踪微分器td用于获得输入的控制目标的至少一个过渡信号。跟踪微分器根据控
制对象的承受能力和控制目标安排合适的过渡过程，使输出的过渡信号能够降低初始阶段对控制对象的冲击，有效解决超调与快速性的矛盾。
[0071]
在一个可能的实施方式中，将控制目标的值输入跟踪微分器，输出过渡信号和过渡信号的微分信号。过渡信号指示角速度向量，过渡信号的微分信号指示角加速度向量。
[0072]
扩张状态观测器eso用于根据控制对象输出的第一姿态计算姿态估计值和扰动量估计值；控制对象输出的第一姿态包括控制对象输出的第一俯仰角、第一横滚角和第一偏航角；姿态估计值至少包括俯仰角估计值、横滚角估计值和偏航角估计值。扩张状态观测器eso对控制对象所受的内扰和外扰进行了动态估值，输出观测信号。
[0073]
在一个可能的实施方式中，将过渡信号和所述过渡信号的微分信号输入控制对象，将控制对象输出的第一姿态输入扩张状态观测器，输出该姿态的估计值和扰动量估计值；第一姿态的估计值包括第一估计值和第二估计值，第二估计值为第一估计值的微分值。第一估计值指示角速度向量估计值，第二估计值指示角加速度向量估计值。
[0074]
非线性状态误差反馈模块nlsef用于计算过渡信号与姿态估计值的误差值，对误差值进行非线性误差反馈，得出第一控制量。非线性状态误差反馈模块对跟踪微分器输出的过渡信号与扩张状态观测器估计输出的观测信号的误差进行非线性误差反馈输出第一控制量。
[0075]
在一个可能的实施方式中，可以将过渡信号和第一估计值进行比较，得到第一误差值；将过渡信号的微分信号和第二估计值进行比较，得到第二误差值；对第一误差值和第二误差值进行非线性误差反馈，得出所述第一控制量。
[0076]
扰动补偿模块用于根据所述扰动量的估计值对第一控制量进行补偿，获得第二控制量。可以根据扩张状态观测器估计出的控制对象受到的扰动量来对第一控制量进行补偿，输出第二控制量给控制对象。
[0077]
在一个可能的实施方式中，可以将扰动量反馈到第一控制量，利用补偿系数对第一控制量进行补偿，输出第二控制量。
[0078]
控制对象根据输入的第二控制量控制输出第二姿态，包括第二俯仰角、第二横滚角和第二偏航角。
[0079]
从式(1)
‑
(3)水下航行器的动力学方程可以看出，水下航行器的俯仰、横滚和偏航三个输出通道相互耦合，是典型的多输入多输出的控制系统。对于推力f为常值的水下航行器，可以近似认为推力f方向上的速度v
f
为一常值，并且水下航行器的推力f方向的速度向量u≈v
f
，水下航行器的姿态系统模型可以简化为：
[0080][0081]
式(10)中，r是横滚角速度，q是俯仰角速度，p是偏航角速度，是横滚角加速度，
是俯仰角加速度，是偏航角加速度，g
r
，g
q
，g
p
，分别为三个通道上的已知项；f
r
，f
q
，f
p
分别为三个通道上的未知项，b1，b2，b3为三个通道上通道上的控制系数。
[0082]
由式(10)表达的姿态系统模型可知，作为控制对象的舵机为二阶函数，需二阶自抗扰控制器对其进行控制。
[0083]
图4为水下航行器的姿态控制系统，如图4所示，横滚角、俯仰角和偏航角三个通道的自抗扰控制器可采用相同的设计方法进行逐一实现。在水下航行器的姿态控制系统中，横滚角、俯仰角和偏航角3个通道上的舵机输出的姿态角是分别通过俯仰、横滚和偏航三个通道的自抗扰控制器来单独控制。
[0084]
在一个可能的实施方式中，本技术的实施例提供一种水下航行器姿态控制方法的控制流程包括以下步骤。
[0085]
s101，根据控制对象的承受能力和水下航行器的动力学模型设置控制对象的控制目标；其中控制对象为舵机，控制目标为以下之一：俯仰角、横滚角和偏航角。
[0086]
在一个可能的实施方式中，可以分别设定第一通道的控制目标x为输入舵机的横滚角的值，横滚角加速度的值为则：
[0087][0088]
设定第二通道的控制目标y为输入舵机的俯仰角θ的值，俯仰角加速度的值为则：
[0089][0090]
设定第三通道的控制目标z为输入舵机的偏航角ψ的值，横滚角加速度的值为则：
[0091][0092]
s102，获得所述控制目标的过渡信号。
[0093]
在一个可能的实施方式中，输入舵机的横滚角可以通过将期望横滚角输入跟踪微分器进行过渡获得过渡信号，如下：
[0094][0095]
式(14)中，k为采样因子序列，取值为1，2，3，...n，n为大于1的自然数；为第k
‑
1次采样时输入跟踪微分器的期望横滚角的信号，跟踪微分器的输出过渡信号为u1(k)、u2(k)；u1(k)是跟踪微分器输出的横滚角过渡信号，u2(k)是u1(k)的微分信号，其中h为积分步长，变量r0为速度因子，h0为滤波因子；fhan(.)为一个非线性函数，定义为：
[0096][0097]
式(15)中，i，j为自变量；r为控制量增益，h为积分步长；d，a0，c，a1，a2，s
c
，s
a
为中间变量；sign(
·
)是符号函数，其功能是去某个数的符号(正或负)，示例性地当x＞0，sign(x)＝1；当x＝0，sign(x)＝0；当x＜0，sign(x)＝
‑
1。
[0098]
同理，输入舵机的俯仰角θ可以通过将期望俯仰角θ
d
输入踪微分器进行过渡如下：
[0099][0100]
式(16)中，将θ
d
(k
‑
1)为第k
‑
1次采样时输入跟踪微分器的期望俯仰角的信号，跟踪微分器的输出过渡信号为u1(k)、v2(k)；v1(k)是跟踪微分器输出的俯仰角过渡信号，v2(k)是v1(k)的微分信号。
[0101]
同理，输入舵机的偏航角ψ可以通过将期望偏航角ψd输入跟踪微分器进行过渡如下：
[0102][0103]
式(17)中，ψ
d
(k
‑
1)为第k
‑
1次采样时输入跟踪微分器的期望偏航角的信号，跟踪微分器的输出过渡信号为w1(k)、w2(k)；w1(k)是跟踪微分器输出的偏航角的过渡信号，w2(k)是w1(k)的微分信号。
[0104]
s103，根据控制对象输出的第一姿态计算姿态估计值和扰动量估计值；其中第一姿态至少包括控制对象输出的第一俯仰角δ
e
、第一横滚角δ
d
和第一偏航角δ
r
。在初始状态下，可以将至少一个过渡信号例如u1(k)、v1(k)或w1(k)输入控制对象以获得控制对象的输出姿态。在工作状态下，将以当前的控制量为输入的控制对象，其输出的第一姿态作为扩张状态观测器的输入，估计控制对象输出的姿态与对应的控制目标的误差和扰动量。
[0105]
在一个可能的实施方式中，可以通过扩张状态观测器估计控制对象的输出姿态的误差和扰动量。其中扩张状态观测器的观测模型为：
[0106]
ε＝x(k)
‑
x(k)
ꢀꢀ
(18)
[0107]
式(18)中，ε为扩张状态观测器的扩张因子，表达的物理意义为第k次采样时控制目标x(k)与输出估计值x(k)的误差值。
[0108]
将横滚舵机的当前输出δ
d
作为扩张状态观测器的输入，扩张状态观测器输出第一通道对应的第一估计值、第二估计值和扰动量估计值为：
[0109][0110]
式(19)中，ε1为扩张状态观测器的扩张因子；x1(k)为第k次采样时扩张状态观测器输出的第一通道第一估计值；x2(k)为扩张状态观测器输出的第一通道第二估计值；x3(k)为扩张状态观测器输出的第一通道扰动量估计值；β
01
，β
02
，β
03
为输出误差校正增益，δ为函数fal(
·
)在原点附近的线性区间宽度；α1＝0.5，α2＝0.25，非线性函数fal(
·
)的定义为：
[0111][0112]
式(20)中，i，α，d均为自变量，取值为任意实数。
[0113]
同理，将俯仰舵机的输出δ
e
作为扩张状态观测器的输入，扩张状态观测器输出第二通道对应的第一估计值、第二估计值和扰动量估计值：
[0114][0115]
式(21)中，y1(k)为扩张状态观测器输出的第二通道第一估计值，y2(k)为扩张状态观测器输出的第二通道第二估计值，y3(k)为对扩张状态观测器输出的第二通道扰动量估计值。
[0116]
同理，将偏航舵机输出的δ
r
作为扩张状态观测器的输入，扩张状态观测器输出第三通道对应的第一估计值、第二估计值和扰动量估计值：
[0117][0118]
式(22)中，z1(k)为扩张状态观测器输出的第三通道第一估计值、z2(k)为扩张状态观测器输出的第三通道第二估计值和z3(k)为对扩张状态观测器输出的三通道扰动量估计
值。
[0119]
s104，对过渡信号及其微分信号与扩张状态观测器输出的第一估计值、第二估计值分别进行比较获得的误差进行非线性误差反馈，得到第一控制量；根据扰动量估计值来对第一控制量进行补偿，得到第二控制量。
[0120]
在一个可以实现可能的实施方式中，可以通过控制量的形成模块(nlsef)进行非线性误差反馈。
[0121]
在一个可能的实施方式中，可以将第一通道的过渡信号及其微分信号与扩张状态观测器输出的第一通道对应的第一估计值、第二估计值分别进行比较获得的误差进行非线性误差反馈，得到第一通道的第一控制量；根据扰动量估计值来对第一通道的第一控制量进行补偿，得到第一通道的第二控制量。
[0122]
示例性地，将u1(k)，u2(k)与扩张状态观测器输出估计值x1(k)，x2(k)进行比较，得到系统误差e1(k)，e2(k)，对e1(k)，e2(k)进行非线性误差反馈，得出横滚角通道的第一控制量u0(k)。将横滚角扰动量估计值x3(k)反馈到控制量u0(k)，除以补偿系数得到横滚角通道的第二控制量u(k)。控制量的形成模型如下：
[0123][0124]
式(24)中，c为阻尼系数，r为控制量增益，h1为精度因子，b
11
为补偿系数。
[0125]
在一个可能的实施方式中，可以将第二通道的过渡信号及其微分信号与扩张状态观测器输出的第二通道对应第一估计值、第二估计值分别进行比较获得的误差进行非线性误差反馈，得到第二通道的第一控制量；根据扰动量估计值来对第二通道的第一控制量进行补偿，得到第二通道的第二控制量。
[0126]
示例性地，可以参考图3，将v1(k)，v2(k)与扩张状态观测器输出估计值y1(k)，y2(k)进行比较，得到系统误差e3(k)，e4(k)，对e3(k)、e4(k)进行非线性误差反馈，得出第二通道的第一控制量v0(k)。将总干扰项的估计值y3(k)反馈到控制量v0(k)，除以补偿系数得到第二控制量v(k)。则第一通道的控制量的形成模型如下：
[0127][0128]
在一个可能的实施方式中，可以将第三通道的过渡信号及其微分信号与扩张状态观测器输出的第三通道对应第一估计值、第二估计值分别进行比较获得的误差进行非线性误差反馈，得到第三通道的第一控制量；根据扰动量估计值来对第三道的第一控制量进行补偿，得到第三通道的第二控制量。
[0129]
示例性地，将w1(k)，w2(k)与扩张状态观测器输出估计值z1(k)，z2(k)进行比较，得到系统误差e5(k)，e6(k)，对e5(k)、e6(k)进行非线性误差反馈，得出控制量w0(k)。将对外部扰动和内部干扰项的估计值z3(k)反馈到控制量w0(k)，除以补偿系数得到控制量估计w(k)。控制量的形成模块如下：
[0130][0131]
s105，将第二控制量输入控制对象，输出下一次控制对象的姿态，以驱动水下航行器。
[0132]
在一个可能的实施方式中，分别将第一通道第二控制量v(k)、第二通道第二控制量u(k)、第三通道第二控制量w(k)分别输入第一舵机、第二舵机和第三舵机，分别输出第二俯仰角、第二横滚角和第二偏航角对无人水下航行器姿态进行控制。
[0133]
本技术实施例提出的水下航行器姿态控制方法针对无人水下航行器控制系统中存在模型不确定性、外界扰动和强耦合的情况，采用了三通道的自抗扰控制器，利用跟踪微分器直接获取设定值的微分信号，进而设计扩张状态观测器对水下航行器姿态控制系统中存在的扰动进行估计，并将扰动估计值与控制器的自抗扰控制算法相结合，使水下航行器姿态收敛到期望值。
[0134]
本技术实施例分别用adrc和pid算法实现水下航行器的姿态控制，对控制器参数进行设置，并根据各种控制器参数整定原理进行整定。
[0135]
首先验证自抗扰控制算法在水下航行器稳定控制中的稳定性。设定水下航行器三个姿态角的初始值为均为0.5rad，控制目标为让三个姿态角均稳定在0度，两种算法的姿态控制仿真结果如图5～图7所示。
[0136]
表1稳定性实验性能指标
[0137][0138]
通过比较分析仿真响应曲线可知，这两中算法均可能实现姿态角的稳定控制，但具体性能指标还是有差距的，如表1所示，其中σ为超调量，t
s
为响应时间，两种算法中pid算法的超调量σ的值和响应时间t
s
的值均大于adrc算法。这是由于在水下航行器中，横滚、俯仰和偏航三通道的模型是非线性的且存在较强的耦合性，其他通道的变化会对当前姿态角产生影响，而pid算法会直接将非线性模型近似为线性从而忽略其耦合，但这并不会消除通道之间的干扰作用；在adrc算法中，跟踪微分器解决了快速性和超调之间的矛盾，抑制了系统响应的超调，同时这种算法将不同通道之间的相互影响当作系统的内部扰动，并对该干
扰进行在线估计和实时补偿，以上便验证了adrc算法具有良好的控制稳定性。
[0139]
然后，验证姿态控制抗干扰性。水下航行器在水下航行的过程中，系统会受到外部扰动发生突变的情况，在3s，6s和6s时刻分别对横滚、俯仰、偏航回路的角加速度加入幅值为20、脉宽为1s的矩形波作为突变信号来测试控制器的抗扰性，在adrc和pid控制器的作用下，两种算法的姿态控制仿真结果如图8
‑
图10所示。
[0140]
在有外部干扰的作用下，adrc和pid控制器都能使姿态角跟随期望姿态角的趋势变化，说明这两种算法都有一定的抗干扰能力。相比之下adrc算法表现最优。在相同的外部干扰下，adrc能很快的回到平衡位置，其抗干扰能力也较好；pid虽然也能回到平衡位置，但无法有效抑制干扰，响应曲线有幅值较大的振动。出现以上原因是因为adrc算法有干扰观测器，能够对外部扰动进行实时估计并补偿。
[0141]
再验证姿态控制鲁棒性。假设初始姿态角为0.5rad的期望姿态角，在保持各算法参数不变的情况下，将水下航行器的质量m和惯性张量分别增大和减小30％，姿态控制仿真结果如图11
‑
图13所示。
[0142]
由仿真结果可知，在各算法参数没有改变的情况下，尽管水下航行器的模型参数有变化，但各通道的动态响应曲线基本没有太大变化。由此可说明adrc算法有一定的鲁棒性，能有效抑制系统参数的不确定性。
[0143]
上述仿真实验验证了adrc算法在水下航行器姿态控制中的有效性，该算法能高效、准确地实现姿态角的稳定跟踪，具有超调小、调节时间短、抗干扰性能好、鲁棒性强的优点。
[0144]
本技术实施例提出的水下航行器姿态控制方法利用自抗扰控制算法对三个姿态进行解耦控制，有效提高了系统的鲁棒性和响应速度；采用扩张观测器对模型中的不确定性进行精确估计，能够把系统内部干扰和外部扰动当做总扰动进行估计，消除扰动对系统的影响，提高了系统抗干扰的能力。
[0145]
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本技术实施例的范围。
[0146]
此外，本技术实施例的各个方面或特征可能成方法、装置或使用标准编程和/或工程技术的制品。本技术中使用的术语“制品”涵盖可从任何计算机可读器件、载体或介质访问的计算机程序。例如，计算机可读介质可以包括，但不限于：磁存储器件(例如，硬盘、软盘或磁带等)，光盘(例如，压缩盘(compact disc，cd)、数字通用盘(digital versatile disc，dvd)等)，智能卡和闪存器件(例如，可擦写可编程只读存储器(erasable programmable read
‑
only memory，eprom)、卡、棒或钥匙驱动器等)。另外，本文描述的各种存储介质可代表用于存储信息的一个或多个设备和/或其它机器可读介质。术语“机器可读介质”可包括但不限于，无线信道和能够存储、包含和/或承载指令和/或数据的各种其它介质。
[0147]
应当理解的是，在本技术实施例的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本技术
实施例的实施过程构成任何限定。
[0148]
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0149]
在本技术所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
[0150]
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
[0151]
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者接入网设备等)执行本技术实施例各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0152]
以上所述，仅为本技术实施例的具体实施方式，但本技术实施例的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术实施例揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本技术实施例的保护范围之内。