[发明专利]一种水下机器人重心辅助调节系统及控制方法

权利要求书

1.一种水下机器人重心辅助调节系统控制方法，其特征在于：水下机器人重心辅助调节系统包括：姿态检测系统、调节运动控制器、丝杠滑块机构、电池舱、水下机器人运动信息输入模块；姿态检测系统用于检测水下机器人本体姿态；水下机器人运动信息输入模块用于检测水下机器人本体的运动控制信息和搭载于水下机器人本体上的水下机械臂的运动控制信息；调节运动控制器根据姿态检测系统检测的水下机器人纵倾姿态信息和水下机器人运动信息输入模块检测的水下机器人运动控制信息和水下机械臂运动控制信息推导出水下机械臂重心变化趋势和大小，进而控制丝杠滑块机构并调整水下机器人的重心，完成目标姿态的控制；丝杠滑块机构是重心辅助调节系统的执行机构，丝杠滑块机构根据调节运动控制器的指令带动电池舱前后移动、调节水下机器人重心，步骤如下：

步骤一：判断水下机器人工作状态，当水下机器人在海流干扰下处于静态情况下，执行步骤二；当水下机器人机械臂重心发生变化情况下，执行步骤三；当水下机器人在既有海流干扰，同时机械臂重心发生变化情况下，执行步骤四；

步骤二：执行水下机器人静态情况下纵倾姿态控制方法，采用自适应积分反演控制方法：

模型参数不确定性和外干扰的水下机器人纵倾运动的运动学和动力学模型如下：

其中θ为纵倾角，q为纵倾角速度，I_y为绕y轴的转动惯量，和M_q|q|为水动力系数,τ_M为推进器推力，中包含模型参数不确定性及外干扰f_l；

为控制器推导方便，将控制器系统改写为如下形式：

其中x₁＝ψ，x₂＝r，ψ为航向角，r为航向角速度，M为惯性项，I_z为自身惯性量，为附加质量，D为水动力参数的比例系数，N_r|r|为水动力参数，u为控制器输出，f_r为外界干扰和模型不确定性；

步骤三：执行水下机械臂重心动态补偿方法，根据水下机械臂重心变化曲线，等效出电池舱重心补偿所需的位移变化曲线，根据水下机械臂水中重量和电池舱重量得到电池舱在规定时间内的变化曲线，根据该曲线，采用PID控制方法调节和控制电池舱运动；

步骤四：执行水下机器人综合动态补偿方法，将步骤二所述的水下机器人静态情况下纵倾姿态控制方法和步骤三所述的水下机械臂重心动态补偿方法结合，结合水下机器人所需的运动姿态、当前推进器速度和方向舵角，计算调节运动控制器的参数，根据参数控制丝杠滑块机构，调节电池舱的位置，实现综合补偿。

2.根据权利要求1所述的一种水下机器人重心辅助调节系统控制方法，其特征在于：所述丝杠滑块机构包括水密电机、联轴器、丝杠、滑块和框架，水密电机输出端通过联轴器与设置在框架内的丝杠连接，电池舱的中部的前后两个位置各固定一个滑块，丝杠与两个滑块相连，水密电机接收调节运动控制器发出的指令带动丝杠旋转，丝杆带动滑块前后移动。

3.根据权利要求1或2所述的一种水下机器人重心辅助调节系统控制方法，其特征在于，框架包括四个导杆和两个平板，电池舱穿过四个导杆并可沿四个导杆移动。