本发明属于水下机器人控制方法，涉及一种基于扩张状态观测器的水下机器人滑模控制方法。
背景技术：
由于空间技术的快速发展，作为验证地面空间技术的必要步骤，微重力仿真技术受到国内外越来越多的广泛关注；水下机器人提供了一个稳定的微重力的环境用来模拟空间中机械设备所处环境，六自由度水下机器人结构之间的耦合性很强，模型的非线性也很高；此外，由于受到水流速度，以及水流粘性阻力等外部扰动的影响，导致比一般机械设备控制难度更高。
当前针对水下机器人的控制策略大多只能获得渐近稳定结果，而且鲁棒性能较差，而对于六自由度水下机器人的强耦合性，强非线性以及复杂的外部扰动，提高系统的鲁棒性能，控制精度以及响应速度具有十分重要的意义；为了进一步提高控制精度以及提高响应速度，同时提高系统的鲁棒性能，我们采用基于扩张状态观测器的滑模控制策略，来获得有限时间稳定结果。
对于传统的自抗扰控制策略，主要包括以下三部分：跟踪微分器，非线性反馈控制律以及扩张状态观测器。跟踪微分器能够合理安排过渡过程，产生给定信号的跟踪信号和微分信号。扩张状态观测器是自抗扰控制器的核心部分，其核心思想是把模型不确定性引起的内部扰动和环境引起的外部扰动都归结为系统的“总扰动”而进行实时估计并给予补偿。非线性反馈控制律是对误差反馈设计非线性组合，从而获得更好的控制效果。同时，非线性的自抗扰控制策略从控制性能上优于线性自抗扰控制方法，但是同样无法获得有限时间稳定结果。同时对于三阶非线性扩张状态观测器参数调节只能凭借经验，并没有理论指导参数调节，给工程应用带来了很大的困难。
滑模控制中系统在滑动模态下与系统参数不确定和外界干扰无关，因此滑模控制能提供快速响应，对系统参数变化和外界干扰不敏感，无需系统辨识等众多优点。但是，一方面一般滑模控制存在的高频抖动不仅破坏了系统的精确性，且会增加系统能量消耗。另一方面控制律的不连续性同样会导致机器人控制力矩的不连续性，从而影响系统性能。
技术实现要素：
本发明提供了一种基于扩张状态观测器的水下机器人滑模控制方法，通过建立惯性坐标系下的水下机器人系统模型，并且设计扩张状态观测器，同时给出扩张状态观测器的参数，设计连续终端滑模控制器对水下机器人进行控制，该方法鲁棒性强，能够获得更高的控制精度和更快的相应速度，便于工程实现。
本发明的技术方案是：一种基于扩张状态观测器的水下机器人滑模控制方法，包括以下步骤：
步骤s1，建立惯性坐标系下水下机器人动力学模型和运动学模型，并且根据其动力学模型和运动学模型建立水下机器人惯性系下运动方程
步骤s2，构建三阶扩张状体观测器：
其中i＝1,2,3,4,5,6和βi1,βi2,βi3为观测器增益，观测误差为ei1(t)＝zi1(t)-ηi(t)，ei2(t)＝zi2(t)-vi(t)，ei3(t)＝zi3(t)-fi(t)，fi(t)为未知扰动；并且通过线性不等式调整观测器的参数；
步骤s3，设计连续终端滑模控制器，首先建立跟踪误差模型，然后基于扩张状态观测器，设计连续终端滑模控制器的控制信号；
步骤s4，得到控制力矩τic(t)，并且控制水下机器人的惯性系下运动方程，从而实现对水下机器人进行控制。
更进一步的，本发明的特点还在于：
其中步骤s1中动力学模型和运动学模型分别为：
其中mrb表示机体惯性矩阵，表示crb机体科里奥氏力矩阵，mam表示与机体相关的水流介质惯性矩阵，cam表示与机体相关的水流介质科里奥氏力矩阵，dr(vr(t))v(t)为粘性阻力，g(η(t))为负浮力，τc(t)为控制力矩，j(η(t))为雅克比矩阵，η(t)，v(t)和vr(t)＝v(t)-vc(t)分别为体坐标系下机体位置，机体速度和体坐标系下相对流体的广义速度，vc(t)为体坐标系下水流的速度。
其中通过线性矩阵不等式调整扩张状态观测器的参数，该线性不等式为：
其中k1～k7是已知正定常数，且满足下式成立：
其中步骤s3中跟踪误差模型为：xi1(t)＝ηi(t)-ηid(t),xi2(t)＝vi(t)-vid(t)。
其中步骤s3中连续终端滑模控制器的控制信号为：
其中ki1,ki2,ti,αi是控制器参数，zi3(t)是扩张状态观测器观测到的未知扰动信息用于实时补偿系统内部的不确定性以及变化的外部扰动。
其中步骤s1中惯性系下运动方程中
与现有技术相比，本发明的有益效果是：本发明通过设计新型的三阶扩张状态观测器，并且通过求解线性矩阵不等式，使得调整扩张状态观测器参数变得简单，解决了三阶非线性扩张状态观测器调整参数困难的问题，便于工程实现；同时设计连续终端滑模控制器，保证控制输入的连续性，减低滑模控制的抖振现象，同时获得有限时间控制结果，得到更好的控制性能；基于新型扩张状态观测器的连续终端滑模控制器，对系统不确定性以及外部扰动实时补偿，能够降低滑模控制增益。
附图说明
图1为本发明的流程图。
具体实施方式
下面结合附图和具体实施例对本发明的技术方案进一步说明。
本发明提供了一种基于扩张状态观测器的水下机器人滑模控制方法，如图1所示，包括以下步骤：
步骤s1，建立惯性坐标系下水下机器人动力学模型和运动学模型，分别为：
其中mrb表示机体惯性矩阵，表示crb机体科里奥氏力矩阵，mam表示与机体相关的水流介质惯性矩阵，cam表示与机体相关的水流介质科里奥氏力矩阵，dr(vr(t))v(t)为粘性阻力，g(η(t))为负浮力，τc(t)为控制力矩，j(η(t))为雅克比矩阵，η(t)，v(t)和vr(t)＝v(t)-vc(t)分别为体坐标系下机体位置，机体速度和体坐标系下相对流体的广义速度，vc(t)为体坐标系下水流的速度。
为了便于设计，对上述的假设如下：体坐标系下的水流速度vc为慢时变的，即vc(t)≈0；vc相对水下机器人的速度v为小量时，近似得到c(v(t))v(t)≈c(vr(t))vr(t)。
因此将公式(1)简化得到
其中m＝mrb+mam，c＝crb+cam。
最终得到惯性系下运动方程为：
其中
相关的参数定义如下：rb＝[xb,yb,zb]t＝[0,0,0]t，rg＝[xg,yg,zg]t＝[0,0,0.05]t，m＝125。
步骤s2，由于水下机器人惯性系下动力学方程为二阶，为估计系统不确定性以及环境外绕，构建三阶扩张状态观测器为：
其中i＝1,2,3,4,5,6，βi1,βi2,βi3为观测器增益。定义观测误差为ei1(t)＝zi1(t)-ηi(t)，ei2(t)＝zi2(t)-vi(t)，ei3(t)＝zi3(t)-fi(t)，fi(t)是未知扰动。
对于未知扰动fi(t)，假设如下：
其中li是已知常数。
针对三阶扩张状态观测器参数调整复杂的问题，本方法通过求解线性矩阵不等式，来调整扩张状态观测器的参数，使扩张状态观测器达到一个良好的估计效果；其中线性矩阵不等式为：
其中k1～k7是已知正定常数，需要保证以下不等式成立。
步骤s3，设计连续终端滑模控制器；为了获得更高的控制精度以及更快的相应速度，同时为了降低滑模控制算法的抖振现象，因此本方法通过使用连续终端滑模控制算法达到有限时间稳定结论；对于水下机器人，以其自由度定义跟踪误差为：xi1(t)＝ηi(t)-ηid(t),xi2(t)＝vi(t)-vid(t)，其中ηid(t)为理想位置信号，vid(t)是理想速度信号。
基于扩张状态观测器，设计如下连续终端滑模控制信号：
其中ki1,ki2,ti,αi是控制器参数，zi3(t)是扩张状态观测器观测到的未知扰动信息用于实时补偿系统内部的不确定性以及变化的外部扰动。
步骤s4，完成水下机器人的控制策略；具体的获得最终控制力矩τic(t)，带入到惯性坐标系下的水下机器人系统模型公式(4)中进行控制，根据控制策略对水下机器人分别设计扩张状态观测器以及连续终端滑模控制器，完成对水下机器人进行控制。
本发明的方法尤其适用于六自由度水下机器人，根据该方法首先建立惯性坐标系下六自由度水下机器人系统模型，即其惯性系下运动方程；然后针对该六自由度水下机器人设计三阶扩张状态观测器，并通过求解线性矩阵不等式给出扩张状态观测器的参数；然后设计连续终端滑模控制器对水下机器人进行控制，不考虑系统的确定性，控制方法鲁棒性强，能够获得更高的控制精度以及更快的响应速度。