本发明属于探测，具体涉及一种未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法。
背景技术：
1、自主水下航行器(autonomous underwater vehicle,auv)在水下综合应用领域中具有广泛的应用前景。在海洋探测领域，auv可以携带各种探测传感器，为科学家提供了丰富的海洋数据，被广泛应用于海洋物理场勘测、海底地形调查、极地冰下探测以及海洋生物探测。在工程检修领域，auv还可应用于海缆巡检作业。
2、在海缆巡检故障点定位时，auv需要通过悬停功能来使其能够稳定停留在特定水深位置，实施详细的观察和使用各种传感器进行高质量数据采集。这包括对海缆物理损伤、磨损等问题的精准检测，同时允许更为精确的定位和记录海缆位置信息。悬停功能在整个海缆巡检任务中极其重要，然而目前针对auv悬停功能的研究较少。为了解决auv的悬停问题，至少需要实现深度、俯仰角、航向角、水平面横向和纵向五个自由度的稳定控制。其中深度、俯仰角和航向角这三个自由度由于检测相对容易且测量精度高，相关的控制方法已经比较成熟，而水平面横向与纵向的定位检测则相对困难。
3、申请号为201811561751.5的发明专利(中国海洋大学的专利)公布了一种基于自抗扰的无模型自适应auv控制方法，克服了传统控制算法快速性和超调性之间的矛盾，大大提高了系统的抗干扰能力。申请号为201910069919.9的发明专利(大连海事大学)公开了一种基于有限时间扩张状态观测器的auv运动控制方法，提高了欠驱动auv的运动控制系统的响应速度及控制精度。
4、现有技术存在以下不足之处：从定位感知方式来看，一般可基于dopplervelocity log(dvl)、惯性导航系统(ins)或视觉进行定位。其中，ins可提供实时的位置和姿态信息且不依赖外界的信号源，但是长时间运行会有较大累计误差。dvl能实时提供auv离底速度，但用来做定位的精度不高。视觉定位通过摄像头感知周围环境，位置精度高但由于摄像头视角的局限性，存在受到扰动后丢失目标的情况。
技术实现思路
1、针对上述背景技术介绍中存在的问题，本发明的目的在于提供了一种未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，该方法基于观测速度进行扰动补偿，使得auv能够在遇到外部干扰导致视觉目标丢失的情况下快速找回目标恢复悬停定位，一定程度上缓解了视觉定位视角局限性带来的不利影响，大大增加了auv执行悬停作业任务的成功率。
2、本发明采用的技术方案是：
3、未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其具体步骤如下：
4、s1，auv的空间悬停定位是对除横摇自由度外的5个自由度进行主动控制，每个自由度都对应着相应的控制输入，引入扩张状态观测器来估计总扰动并在控制回路总实时消除扰动；
5、其中，auv在各个自由度的动力学方程为：
6、
7、其中，y表示输出量，可以是位移或者角度，表示其中一个自由度的加速度或角加速度；u为对应自由度的控制输入，fw为包括系统参数中未知部分的总扰动，b0为系统参数中已知部分；
8、扩张状态观测器的方程为：
9、
10、其中，z1为位移的观测值，为观测位移的变化率；z2为速度的观测值，为观测速度的变化率；z3为扰动的观测值，为观测扰动的变化率；ωo为观测器带宽，与auv的观测噪声有关；
11、s2，当在未知扰动下视觉目标丢失时，保留丢失目标前最后一个时刻的观测值y0作为扩张状态观测器的输入，将目标丢失后无法观测到的位移变化量设为δy，此刻扩张状态观测器方程为：
12、
13、此时，总扰动z3的变化率并且存在来不及观测到的扰动fd，fd对系统的影响可以等效到输入端，即等效为在输入端存在一个还未被观测到的扰动ud，此时，auv的动力学方程为：
14、
15、其中表示其中一个自由度的加速度或角加速度，fo为已观测到的扰动，u0为期望的控制量，由f0的定义可知z3＝f0；
16、s3，在auv失去目标时，auv的瞬时速度反应了外界扰动的变化趋势，此时将扩张状态观测器的观测速度乘以一个补偿系数用来对扰动进行补偿，并将该补偿量反映到控制量u中去，即新的控制量u可以写成：
17、
18、其中e为期望值；kp＝ωc2为比例系数；kd＝-2ωc为微分比例系数；ωc为控制器调整参数；kh为需要根据实际情况调整的补偿系数；ud为还未被观测到的扰动，可以近似认为δt为单位时间，当ud＝khz2时，扰动被完全抵消。
19、进一步，步骤s1中auv在各个自由度的动力学方程获得步骤如下：
20、对任何一个自由度进行单独的控制，以纵向运动为例，设外部扰动为w，根据粘性水动力矩阵变换并考虑外部扰动可以得到纵荡自由度的动力学方程为：
21、
22、式中，m为auv质量，g和b分别表示重力和浮力，x、y、z为体坐标系下的纵向、横向、垂向的力，u、v、w为体坐标系下纵向、横向、垂向的速度，为体坐标系下纵向的加速度，|u|为体坐标系下纵向速度的绝对值，q、r为体坐标系下俯仰和回转方向的角速度，为各个方向上的水动力系数，xu|u|是体坐标系下对应方向的二次水动力系数，θ为auv的俯仰角；纵荡自由度的动力学方程在惯性坐标系中近似写成如下二阶微分方程的形式：
23、
24、用同样的方法，其他自由度的动力学方程都可以写成如上式所示的二阶微分方程形式，其中，ξ为惯性坐标系下的纵向位移，为惯性坐标系下的纵向速度，为惯性坐标系下的纵向加速度，a1,a2为未知参数，a0包含了重力、浮力以及其他自由度的影响；
25、auv在各个自由度的动力学方程都可以进一步化简为如下的通用形式：
26、
27、式中，为总扰动，b为系统参数，包含了已知的b0和未知的部分；其中b的未知部分可以并入总扰动f，从而得到auv在各个自由度的动力学方程：
28、
29、其中，fw为包含了b的未知部分的总扰动。
30、进一步，步骤s1中扩张状态观测器的方程获得步骤如下：
31、在纵荡和横荡自由度中，x、y分别代表纵向位移和横向位移，这两个量通过视觉识别测量到；选取状态变量：x1＝y，x3＝f，其二阶状态空间方程为：
32、
33、其中，c＝[1 0 0]，b0为b的已知部分，表示速度，表示扰动的变化率；
34、运用龙伯格观测器可以得到扩张状态观测器方程：
35、
36、其中为观测的状态变量和位移，为观测的状态变量的变化率，整理可得：
37、
38、其中，可以写成如下形式：
39、
40、进一步，步骤s3中实际的调参按以下步骤进行：
41、s31，先在静水环境中进行悬停定位实验，初步选择一个较小的ωo，将系统参数b0从小到大调整到合适值，如果没有达到要求，逐步增加ωo，再次将参数b0从小到大调整到合适值，整个过程保持ωc为ωo的0.25倍，直至满足静水中的悬停定位要求；
42、s32，添加小扰动来调整控制器的参数ωc，直至auv能够在较短时间内恢复稳定控制；
43、s33，最后加大扰动，直至auv的视觉丢失目标，再慢慢从小到大调整kh，当补偿量khz2跟上总扰动的变化率后，auv可以逐渐找回目标。
44、进一步，步骤s33中会加大补偿系数kh值对扰动进行过补偿，从而控制auv快速沿着外部扰动的反方向运动，进而让auv快速找回丢失的视觉目标。
45、进一步，当视觉目标没有丢失时，kh赋值为0。
46、本发明与现有技术相比，其显著优点包括：使得auv在未知外部扰动下丢失目标后能快速找回，提高了基于视觉定位悬停控制方法的可靠性和稳定性，还可用于需要跟踪目标快速移动的应用场景。
1.未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其具体步骤如下：
2.根据权利要求1所述的未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其特征在于：步骤s1中auv在各个自由度的动力学方程获得步骤如下：
3.根据权利要求2所述的未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其特征在于：步骤s1中扩张状态观测器的方程获得步骤如下：
4.根据权利要求1所述的未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其特征在于：步骤s3中实际的调参按以下步骤进行：
5.根据权利要求4所述的未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其特征在于：步骤s33中会加大补偿系数kh值对扰动进行过补偿，从而控制auv快速沿着外部扰动的反方向运动，进而让auv快速找回丢失的视觉目标。
6.根据权利要求1所述的未知扰动下基于视觉伺服的水下或冰下探测auv悬停控制方法，其特征在于：当视觉目标没有丢失时，kh赋值为0。
