本发明涉及一种水下机器人重心辅助调节系统，也涉及一种纵倾姿态的控制方法。
背景技术：
随着海洋开发的迅猛发展，无人自主式水下机器人越来越多的被应用到海洋开发和探测过程中，成为国内外研究机构的研究热点。为了节约系统能力，航时更长、航行更远，无人自主式水下机器人一般设计成欠驱动形式，即单推进器加尾部十字舵的形式。由于局部自由度的缺失，使得水下机器人在受到水下海流干扰、自身携带的水下机械臂运动产生的扰动时，为了维持自身的运动稳定性，需要航行一段路程，通过运动抵消外干扰对水下机器人本体产生的影响，这无疑破坏原有的运动状态和预定的轨迹规划。
针对此问题，国内外开展许多有益的研究工作。武建国发明了一种水下机器人浮力调节装置，利用内外皮囊实现浮力调整，进而调整水下机器人姿态，但这种方式改变水下机器人的重力和浮力的对比关系，必然要牺牲水下机器人在升沉方向的位置才能实现自身稳定问题。谢少荣发明了一种水下机器人姿态主动调节系统，该系统包含一个滑动平台和一个转动平台，通过附加负载的变化改变水下机器人重心位置进而补偿外干扰影响，具有很重要的借鉴意义。但该发明额外增加了水下机器人的负担，没有有效利用水下机器人自身设备，由于水下机器人结构紧凑，因而造成了水下机器人空间上的浪费。唐智杰等人也发明了一种水下机器人姿态内控制系统。它包括：一个主控制器连接调节机构、一个姿态传感器和一个上位机控制器，所述调节机构是一个安装在水下机器人内部的平衡调节机构，该机构包括重心滑块、双轴驱动电机、蜗杆、导轨、滑杆、限位开关和机构平台。该发明与谢少荣发明的水下机器人姿态主动调节系统类似，牺牲了水下机器人本体内部有效空间，且没有针对具体外干扰情况提出有效的解决方案和控制方法。
技术实现要素：
针对上述现有技术，本发明要解决的技术问题是提供一种节约水下机器人本体内部有效空间且考虑具体外干扰情况的单自由度的调节水下机器人前进方向的重心位置的水下机器人重心辅助调节系统及控制方法。
为解决上述技术问题，本发明一种水下机器人重心辅助调节系统，包括：姿态检测系统、调节运动控制器、丝杠滑块机构、电池舱、水下机器人运动信息输入模块；姿态检测系统用于检测水下机器人本体姿态；水下机器人运动信息输入模块用于检测水下机器人本体的运动控制信息和搭载水下机器人本体上的水下机械臂的运动控制信息；调节运动控制器根据姿态检测系统检测的水下机器人纵倾姿态信息和水下机器人运动信息输入模块检测的水下机器人运动控制信息和水下机械臂运动控制信息推导出水下机械臂重心变化趋势和大小，进而控制丝杠滑块机构并调整水下机器人的重心，完成目标姿态的控制；丝杠滑块机构是重心辅助调节系统的执行机构，丝杠滑块机构根据调节运动控制器的指令带动电池舱前后移动、调节水下机器人重心。
本发明一种水下机器人重心辅助调节系统，还包括：
1.丝杠滑块机构包括水密电机、联轴器、丝杠、滑块和框架，水密电机输出端通过联轴器与设置在框架内的丝杠连接，电池舱的中部的前后两个位置各固定一个滑块，丝杠与两个滑块相连，水密电机接收调节运动控制器发出的指令带动丝杠旋转，丝杆带动滑块前后移动。
2.框架包括四个导杆和两个平板，电池舱穿过四个导杆并可沿四个导杆移动。
本发明一种用于上述的水下机器人重心辅助调节系统的控制方法，包括以下步骤：
步骤一：判断水下机器人工作状态，当水下机器人在海流干扰下处于静态情况下，执行步骤二；当水下机器人机械臂重心发生变化情况下，执行步骤三；当水下机器人在既有海流干扰，同时机械臂重心发生变化情况下，执行步骤四；
步骤二：执行水下机器人静态情况下纵倾姿态控制方法，采用自适应积分反演控制方法：
模型参数不确定性和外干扰等的水下机器人纵倾运动的运动学和动力学模型如下：
其中θ为纵倾角，q为纵倾角速度，iy为绕y轴的转动惯量，
为控制器推导方便，将控制器系统改写为如下形式：
其中x1＝ψ，x2＝r，
步骤三：执行水下机械臂重心动态补偿方法，根据水下机械臂重心变化曲线，等效出电池舱重心补偿所需的位移变化曲线，根据水下机械臂水中重量和电池舱重量得到控制辅助调节系统的电池舱在规定时间内的变化曲线，根据该曲线，采用pid控制方法调节和控制电池舱运动；
步骤四：执行水下机器人综合动态补偿方法，将步骤二所述的水下机器人静态情况下纵倾姿态控制方法和步骤三所述的水下机械臂重心动态补偿方法结合，结合水下机器人所需的运动姿态、当前推进器速度和方向舵角，计算调节运动控制器的参数，根据参数控制丝杠滑块机构，调节电池舱的位置，实现综合补偿。
本发明的有益效果：本发明为水下机器人水下抓取物体时重心和姿态控制提供新的解决方案，同时为水下机器人水下作业的稳定性提供了保证。
(1)本发明有效解决水下机器人水中静态条件下，抵抗海流干扰问题，使得水下机器人能够实现保持位置不变情况下，姿态自动恢复；
(2)本发明能够补偿水下机械臂运动过程中，重心变化对水下机器人本体的干扰，减少水下机械臂和水下机器人之间的动力学耦合；
(3)本发明实现了综合海况条件下，水下机器人纵倾方向的姿态稳定问题，通过多种因素的耦合满足水下机器人运动控制所需的稳定性问题；
(4)本发明结构紧凑、空间利用合理，充分利用水下机器人结构上的特点，将本体中较重的电池舱作为调节单元，解决海流外干扰、水下机械臂重心变化对机器人本体影响；
(5)该发明为水下机器人运动控制提供有效借鉴方案，具有重要的实际工程意义和理论价值。
附图说明
图1为重心辅助调节系统示意图；
图2为丝杠滑块机构组成示意图；
图3为水下机器人垂直面控制方框图；
图4为水下机械臂重心变化曲线；
图5为电池舱等效补偿变化曲线；
图6为水下机器人综合动态补偿控制结构；
图7为水下机器人纵倾角的控制方法框图。
具体实施方式
实施1：如图1所示，系统主要由姿态检测系统、调节运动控制器、丝杠滑块机构、电池舱、水下机器人运动信息输入模块组成：姿态检测系统用于检测水下机器人本体姿态，特别是水下机器人的纵倾信息；调节运动控制器根据当前水下机器人纵倾姿态信息、水下机器人运动控制信息、水下机械臂运动控制信息进行综合判断，控制重心辅助调节系统，调整水下机器人的重心，实现目标姿态的控制；丝杠滑块机构是重心辅助调节系统的执行机构，由水密电机、联轴器、丝杠、滑块和导向机构组成；电池舱是水下机器人重量较大的部分，密度最集中的部分，作为水下机器人重心调节的主要载体；水下机器人运动信息输入模块用于检测水下机器人本体的运动控制信息，如各个推进器运动数据、调节舵的舵角变化信息，同时也要搭载水下机器人本体上的水下机械臂的运动控制信息，进而推导出水下机械臂重心变化趋势和大小，这两种信息的变化为重心辅助调节系统的实时准确控制提供帮助。
实施2：如图2所示，框架1起整体支撑作用，水密电机7通过联轴器6与丝杠5连接，电池舱3的中部的前后两个位置各固定一个滑块4，丝杠5与滑块4相连。水密电机7接收调节运动控制器发出的指令带动丝杠5旋转，丝杆5带动滑块4前后移动，进而实现电池舱3的前后移动。在电池舱3的四角安放四个导杆2，四个导杆2对电池舱3起到支撑和导向的作用。
实施3：模型参数不确定性和外干扰等的水下机器人纵倾运动的运动学和动力学模型如下：
其中θ为纵倾角，q为纵倾角速度，iy为绕y轴的转动惯量，
为控制器推导方便，将控制器系统改写为如下形式：
其中x1＝ψ，x2＝r，
如图3所示为水下机器人垂直面控制方框图，zd为纵倾角，ud为外干扰，δs为推进器推力根据水下机器人静态条件采用纵倾姿态调整方法，以下是纵倾姿态调整法的推导流程：
step1非线性干扰观测器设计
由于水下机器人的运动受到未建模参数不确定性以及外干扰等因素的影响较大，利用非线性观测器来观测这些扰动项是一个很好的办法。假设非线性干扰观测器的形式如下：
其中e为ndo(非线性干扰观测器)的内部变量，
上式中假设观测量的变化缓慢，即
取输出增益l(x1,x2)＝a＞0，则可以解得
将ndo的输出通过输出增益，并且转化到动力学方程中，得到新的水下机器人纵倾运动方程：
其中
step2积分反演控制器设计
对于系统(5)(即式(5)系统)来说，其中包含有观测器未能观测出的扰动项。这就需要为其设计一个鲁棒性较强的控制器，本文采用的积分反演控制器设计过程如下：
假设：系统的期望轨迹具有二阶导数，且δ有界。
定义跟踪误差：
z1＝x1-x1d(6)
对上式求导得：
定义lyapunov函数为：
对式(8)求导数可得：
为了使lyapunov函数的导数负定，取含有误差积分项的虚拟控制量：
其中
定义虚拟控制误差：
z2＝x2-x2d(11)
将式(10)和(11)带入到式(7)中，可以得到：
对式(11)求导可得：
取新的lyapunov函数：
其中
step3自适应律设计
式(15)中包含了经过观测器观测的不确定性和外干扰的残余量δ，而在实际操作过程中，这些残余量的大小是不能被事先知晓的。为了解决这个问题，本文采取自适应的方法，在线估计这些残余量。具体步骤如下：
定义误差量：
假设δ变化缓慢，则：
定义新的lyapunov函数：
则式(18)的导数为：
取控制律和自适应律分别为：
将上式代入到式(19)中，得：
由上可知，v正定，
则系统(2)(即式(2)系统)的控制律及自适应律分别为：
实施4：如图4所示，水下机械臂在水中运动缓慢，主要考虑其重心变化趋势，根据该变化曲线，等效出电池舱进行重心补偿所需的位移变化曲线(用电池舱滑动改变重心来补偿机械臂运动所造成的重心偏移)，如图5所示，以水下机械臂水中重量5kg，电池舱重量10kg为例，得到控制辅助调节系统的电池舱在规定时间内的变化曲线，根据该曲线，调节和控制电池舱运动，采用传统的pid即可实现，这里不过多复述。
实施5：水下机器人在水下动态条件下，较为复杂，既有海流干扰、也有机械臂运动重心产生的变化，同时也有水下机器人运动所需的姿态要求。
如图6所示，综合水下机器人静态情况下纵倾姿态调整方法、水下机械臂重心动态补偿两种方法，结合水下机器人所需的运动姿态、当前推进器速度和方向舵角，计算调节运动控制器的参数，根据参数控制丝杠滑块机构，调节电池舱的位置，实现综合补偿。如图7所示为水下机器人纵倾角的控制方法及重心辅助调节系统。根据不同环境和条件下采用不同的控制方法，实现水下机器人姿态的调节，提高水下机器人的稳定性。
本发明具体实施方式还包括：
水下机器人重心辅助调节系统主要由姿态检测系统、调节运动控制器、丝杠滑块机构、电池舱、水下机器人运动信息输入模块组成：姿态检测系统用于检测水下机器人本体姿态，特别是水下机器人的纵倾信息；调节运动控制器根据当前水下机器人纵倾姿态信息、水下机器人运动控制信息、水下机械臂运动控制信息进行综合判断，控制重心辅助调节系统，调整水下机器人的重心，实现目标姿态的控制；丝杠滑块机构是重心辅助调节系统的执行机构，由水密电机、联轴器、丝杠、滑块和导向机构组成；电池舱是水下机器人重量较大的部分，密度最集中的部分，作为水下机器人重心调节的主要载体；水下机器人运动信息输入模块用于检测水下机器人本体的运动控制信息，如各个推进器运动数据、调节舵的舵角变化信息，同时也要搭载水下机器人本体上的水下机械臂的运动控制信息，进而推导出水下机械臂重心变化趋势和大小，这两种信息的变化为重心辅助调节系统的实时准确控制提供帮助。
水下机器人重心辅助调节系统机械结构如下：
水密电机通过联轴器与丝杠连接，电池舱的中部的前后两个位置各固定一个滑块，丝杠与滑块相连。水密电机接收调节运动控制器发出的指令带动丝杠旋转，丝杆带动滑块前后移动，进而实现电池舱的前后移动。在电池舱的四角安放四个导杆，四个导杆对电池舱起到支撑和导向的作用。
该控制方法分为三种情况：水下机器人静态情况下纵倾姿态调整方法、水下机械臂重心动态补偿方法、水下机器人综合动态补偿方法。
(1)水下机器人静态情况下纵倾姿态调整方法
水下机器人属于欠自由度的运动体，当静止在水下某一位置进行观察作业时，受到某一海流干扰，其姿态会发生变化，如不加控制，水下机器人姿态会自动恢复，但周期较长，且位置会发生变化，偏离原来的位置。水下机器人调整姿态通过推进器和舵角的动态组合，实现姿态调整，这种调整方式往往牺牲水下机器人的空间位置。
由于海流是随机发生，且流速和产生时间不定。为此，采用自适应积分反演控制方法。
模型参数不确定性和外干扰等的水下机器人纵倾运动的运动学和动力学模型如下：
其中θ为纵倾角，q为纵倾角速度，iy为绕y轴的转动惯量，
为控制器推导方便，将控制器系统改写为如下形式：
其中x1＝ψ，x2＝r，
(2)水下机械臂重心动态补偿方法
水下机械臂在水下作业时，其重心会发生短暂变化，影响水下机器人整体重心发生变化，进而影响水下机器人本体姿态发生变化。但是水下机械臂的轨迹规划是已知的，其重心变化是有规律的、可预判的，为此可通过控制重心辅助调节系统补偿水下机械臂重心变化。
水下机械臂在水中运动缓慢，主要考虑其重心变化趋势，根据该变化曲线，等效出电池舱进行重心补偿所需的位移变化曲线，根据水下机械臂水中重量和电池舱重量，可得到控制辅助调节系统的电池舱在规定时间内的变化曲线，根据该曲线，调节和控制电池舱运动，采用传统的pid即可实现。
(3)水下机器人综合动态补偿方法
水下机器人在水下动态条件下，较为复杂，既有海流干扰、也有机械臂运动重心产生的变化，同时也有水下机器人运动所需的姿态要求。
综合水下机器人静态情况下纵倾姿态调整方法、水下机械臂重心动态补偿两种方法，结合水下机器人所需的运动姿态、当前推进器速度和方向舵角，计算调节运动控制器的参数，根据参数控制丝杠滑块机构，调节电池舱的位置，实现综合补偿。